1
$\begingroup$

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

class HeadingKF(object):
    '''
    State: [h, w]
    heading, angular speed
    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? enter image description here

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.