Implementing Kalman Filter for Vehicle Odometry Correction

Question

I am working on estimating a vehicle position using odometry data. Below is the code that I have written to predict vehicle location with input data of yaw rate, steering wheel angle, and vehicle speed. The code takes in dt (not consistent), updated heading angle, x position and y position to update the measurements.

I have currently compared the trajectory generated from this code against the reference trajectory plotted using GPS data, which is showing an error.

I would like to implement Kalman filter for this odometry data, but I am lost on what to set up for my state matrix and state transition matrix. If I set state transition matrix as x_pos, y_pos, heading at k+1, what should I use as state matrix? Simply using state matrix of x,y,yaw, x_speed, y_speed, yaw_rate seems impossible since most of these data are constantly being updated.

Please help!

''' def convert_odometry_info_to_cartesian(odo_yaw_rate, odo_SAS_angle, odo_speed, dt, heading, x_pos, y_pos):

# Declare constants
SOF_S_W_RATIO = 17.0
SOF_L_R = 1.5155
SOF_L_F = 1.4945

# Convert units
odo_yaw_rate = np.deg2rad(odo_yaw_rate) # converted to deg/s
odo_SAS_angle = np.deg2rad(odo_SAS_angle) # coverted to deg
heading = np.deg2rad(heading)

# Based on bicycle model
abs_yaw_rate = abs(odo_yaw_rate)
sin_heading = np.sin(heading)
cos_heading = np.cos(heading)
sin_heading_wt = np.sin(odo_yaw_rate * dt)
cos_heading_wt = np.cos(odo_SAS_angle * dt)

# Calculate wheel angle
wheel_angle = odo_SAS_angle / SOF_S_W_RATIO 
# Calculate slip angle 
slip_angle = np.arctan(SOF_L_R * np.tan(wheel_angle) / (SOF_L_F + SOF_L_R))

# Discrete time velocity model
x_speed = odo_speed * np.cos(slip_angle) 
y_speed = odo_speed * np.sin(slip_angle)

# Discrete time position update models (constant velocity & turn rate model)

x_pos = x_pos + (x_speed / odo_yaw_rate) * (sin_heading *
    cos_heading_wt + cos_heading * sin_heading_wt - sin_heading)
y_pos = y_pos + (x_speed/odo_yaw_rate) * (cos_heading - cos_heading
    * cos_heading_wt + sin_heading * sin_heading_wt)
heading = heading + odo_yaw_rate * dt

# Convert units
heading = np.rad2deg(heading)
slip_angle = np.rad2deg(slip_angle)

return x_pos, y_pos, heading, slip_angle

'''