Реализация фильтра Калмана для коррекции одометрии транспортного средства

Question

Я работаю над оценкой положения автомобиля с использованием данных одометрии. Ниже приведен код, который я написал для прогнозирования местоположения транспортного средства с входными данными о скорости рыскания, углом поворота рулевого колеса и скорости транспортного средства. Код принимает dt (непоследовательно), обновленный угол курса, положение x и положение y для обновления измерений.

Я в настоящее время по сравнению с траекторией генерируемой из этого кода на ссылку траектория нанесена с использованием данных GPS, который показывает ошибку.

Я хотел бы реализовать фильтр Калмана для этих данных одометрии, но я не знаю, что настроить для моей матрицы состояний и матрицы перехода состояний. Если я установил матрицу перехода состояний как x_pos, y_pos, с заголовком k+1, что мне следует использовать в качестве матрицы состояний? Простое использование матрицы состояний x,y,yaw, x_speed, y_speed, yaw_rate кажется невозможным, поскольку большая часть этих данных постоянно обновляется.

Пожалуйста помоги!

'''def convert_odometry_info_to_cartesian(odo_yaw_rate, odo_SAS_angle, odo_speed, dt, заголовок, 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

'''