# Kalman filter for heading estimation provides oscillating output

I am designing a Kalman filter for heading estimation in 2D using magnetic compass, gyroscope and wheel encoder. The system state is $$X = [h, w]$$ and the measurements are $$Z = [h_{mag}, g_z,d_h]$$, where $$h_{mag}$$ is the heading computed with the compass, $$g_z$$ is the angular velocity measured with the gyroscope on the z axis and $$d_h$$ is the angle variation computed using the odometry equation:

$$d_{left} = vl*dt$$ $$d_{right} = vr*dt$$ $$d_h = (dleft - dright) / (wheel\_distance)$$

The state matrix is $$A = \begin{bmatrix}1&dt\\0&1\end{bmatrix}$$

The measurement matrix is $$H = \begin{bmatrix}1&0\\0&1\\0&dt\end{bmatrix}$$

My current implementation is as follows. Please note that my magnetometer is mounted with the X-axis onward and I'm using a reference system with 0° = north and clockwise increments from 0 to 359.

import numpy as np
PI = np.pi

'''
State: [h, w]
Sensors: mag x y, gyr z, enc vl vr
'''
def __init__(self, h_deg, w_deg):
self.wheel_distance = 1

h = h_deg*PI/180
w = w_deg*PI/180

# State variables
self.X = np.array([h, w])
self.state_dim = len(self.X)
self.A = np.eye(self.state_dim) # 2x2

# Initial covariance
self.P = np.eye(self.state_dim)

# Process noise
self.Q = np.eye(self.state_dim)*0.1

# Measurement noise
self.R = np.eye(3) * 0.1

# Measurement matrix (n sensors x state dim)
# mag x and mag y are treated as one sensor (arctan output)
# encoder values are treated as one sensor (odometry output)
self.H = np.zeros((3, self.state_dim)) # mag h, gz, enc dh

def update(self, mx, my, gz_deg, vl, vr, dt):

# Prediction

# h = h0 + w dt
self.A[0, 1] = dt

self.X = np.dot(self.A, self.X)
self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q

# Update

# mag measurement
tmp = np.arctan2(mx, my) # measured h
if mx >= 0 and my >= 0:
tmp = 2*PI - tmp
elif mx >= 0 and my < 0:
tmp = -tmp
elif mx < 0 and my < 0:
tmp = -tmp
elif mx < 0 and my >= 0:
tmp = 2*PI - tmp
tmp += PI/2

# wrap
tmp = wrap(tmp) # will go from 0 to 2pi
mag_h = tmp

# gyr measurement
gz = gz_deg*PI/180

# enc measurement
dleft = vl*dt
dright = vr*dt
dangle = (dleft - dright) / (2*self.wheel_distance)

# build H

# mag_h = h
self.H[0,0] = 1

# gz = w
self.H[1, 1] = 1

# d_angle = w*dt ??
self.H[2, 1] = dt

K = np.dot(np.dot(self.P, self.H.T),
np.linalg.inv(np.dot(np.dot(self.H, self.P),
self.H.T) + self.R))

Z = np.array([mag_h, gz, dangle])
innovation = Z - np.dot(self.H, self.X)

# innovation[0] must be wrapped from -180 to 180
innovation[0] = (innovation[0] + PI) % 2*PI - PI

self.X = self.X + np.dot(K, innovation)
self.X[0] = wrap(self.X[0])
self.P = np.dot((np.eye(self.state_dim) - np.dot(K, self.H)), self.P)

return self.X


As the image below shows, I have a very high oscillation in my output, despite the gyroscope and magnetometer providing a very similar heading computation on their own.

Is this problem related to the tuning of the noise matrices, or are there errors in my matrices and equations?