opencda.core.sensing.localization package

Submodules

opencda.core.sensing.localization.coordinate_transform module

Functions to transfer coordinates under different coordinate system

opencda.core.sensing.localization.coordinate_transform.geo_to_transform(lat, lon, alt, lat_0, lon_0, alt_0)

Convert WG84 to ENU. The origin of the ENU should pass the geo reference. Note this function is a writen by reversing the official API transform_to_geo.

Parameters:

  • lat (float) – current latitude.

  • lon (float) – current longitude.

  • alt (float) – current altitude.

  • lat_0 (float)) – geo_ref latitude.

  • lon_0 (float) – geo_ref longitude.

  • alt_0 (float) – geo_ref altitude.

Returns:

  • x (float) – The transformed x coordinate.

  • y (float) – The transformed y coordinate.

  • z (float) – The transformed z coordinate.

opencda.core.sensing.localization.kalman_filter module

Use Kalman Filter on GPS + IMU for better localization. Reference: https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

class opencda.core.sensing.localization.kalman_filter.KalmanFilter(dt)

Bases: object

Kalman Filter implementation for gps and imu.

Parameters:

dt (float) – The step time for kalman filter calculation.

Q

predict state covariance.

Type:

numpy.array

R

Observation x,y position covariance.

Type:

numpy.array

time_step

The step time for kalman filter calculation.

Type:

float

xEst

Estimated x values.

Type:

numpy.array

PEst

The estimated P values.

Type:

numpy.array

motion_model(x, u)

Predict current position and yaw based on previous result (X = F * X_prev + B * u).

Parameters:

  • x (np.array) – [x_prev, y_prev, yaw_prev, v_prev], shape: (4, 1).

  • u (np.array) – [v_current, imu_yaw_rate], shape:(2, 1).

Returns:

x – Predicted state.

Return type:

np.array

observation_model(x)

Project the state matrix to sensor measurement matrix.

Parameters:

x (np.array) – [x, y, yaw, v], shape: (4. 1).

Returns:

z – Predicted measurement.

Return type:

np.array)

run_step(x, y, heading, velocity, yaw_rate_imu)

Apply KF on current measurement and previous prediction.

Parameters:

  • x (float) – x(esu) coordinate from gnss sensor at current timestamp

  • y (float) – y(esu) coordinate from gnss sensor at current timestamp

  • heading (float) – heading direction at current timestamp.

  • velocity (float) – current speed.

  • yaw_rate_imu (float) – yaw rate rad/s from IMU sensor.

Returns:

Xest – The corrected x, y, heading, and velocity information.

Return type:

np.array

run_step_init(x, y, heading, velocity)

Initial state filling.

Parameters:

  • x (float) – The x coordinate.

  • y (float) – The y coordinate.

  • heading (float) – The heading direction.

  • velocity (float) – The velocity speed.

opencda.core.sensing.localization.localization_debug_helper module

Visualization tools for localization

class opencda.core.sensing.localization.localization_debug_helper.LocDebugHelper(config_yaml, actor_id)

Bases: object

This class aims to help users debugging their localization algorithms. Users can apply this class to draw the x, y coordinate trajectory, yaw angle and vehicle speed from GNSS raw measurements, Kalman filter, and the groundtruth measurements. Error plotting is also enabled.

Attributes
show_animationboolean

Indicator of whether to visulize animtion.

x_scalefloat

The scale of x coordinates.

y_scalefloat

The scale of y coordinates.

gnss_xlist

The list of recorded gnss x coordinates.

gnss_ylist

The list of recorded gnss y coordinates.

gnss_yawlist

The list of recorded gnss yaw angles.

gnss_speedlist

The list of recorded gnss speed values.

filter_xlist

The list of filtered x coordinates.

filter_ylist

The list of filtered y coordinates.

filter_yawlist

The list of filtered yaw angles.

filter_speedlist

The list of filtered speed values.

gt_xlist

The list of ground truth x coordinates.

gt_ylist

The list of ground truth y coordinates.

gt_yawlist

The list of ground truth yaw angles.

gt_speedlist

The list of ground truth speed values.

hxEstlist

The filtered x y coordinates.

hTruelist

The true x y coordinates.

hzlist

The gnss detected x y coordinates.

actor_idint

The list of ground truth speed values.

evaluate()

Plot the localization related data points.

Returns:

The plot of localization related figures. -perform_txt(txt file): The localization related datas saved as text file.

Return type:

-figures(matplotlib.pyplot.plot)

run_step(gnss_x, gnss_y, gnss_yaw, gnss_spd, filter_x, filter_y, filter_yaw, filter_spd, gt_x, gt_y, gt_yaw, gt_spd)

Run a single step for DebugHelper to save and animate(optional) the localization data.

Parameters:

  • -gnss_x (float) – GNSS detected x coordinate.

  • -gnss_y (float) – GNSS detected y coordinate.

  • -gnss_yaw (float) – GNSS detected yaw angle.

  • -gnss_spd (float) – GNSS detected speed value.

  • -filter_x (float) – Filtered x coordinates.

  • -filter_y (float) – Filtered y coordinates.

  • -filter_yaw (float) – Filtered yaw angle.

  • -filter_spd (float) – Filtered speed value.

  • -gt_x (float) – The ground truth x coordinate.

  • -gt_y (float) – The ground truth y coordinate.

  • -gt_yaw (float) – The ground truth yaw angle.

  • -gt_spd (float) – The ground truth speed value.

opencda.core.sensing.localization.localization_manager module

Localization module

class opencda.core.sensing.localization.localization_manager.GnssSensor(vehicle, config)

Bases: object

The default GNSS sensor module.

Parameters:

  • vehicle (carla.Vehicle) – The carla.Vehicle. We need this class to spawn our gnss and imu sensor.

  • config (dict) – The configuration dictionary of the localization module.

sensor

The current sensor actors that will be attach to the vehicles.

Type:

CARLA actor

class opencda.core.sensing.localization.localization_manager.ImuSensor(vehicle)

Bases: object

Default ImuSensor module.

Parameters vehicle : carla.Vehicle

The carla.Vehicle. We need this class to spawn our gnss and imu sensor.

Attributes world : carla.world

The caral world of the current vehicle.

blueprintcarla.blueprint

The current blueprint of the sensor actor.

weak_selfopencda Object

A weak reference point to avoid circular reference.

sensorCARLA actor

The current sensor actors that will be attach to the vehicles.

class opencda.core.sensing.localization.localization_manager.LocalizationManager(vehicle, config_yaml, carla_map)

Bases: object

Default localization module.

Parameters vehicle : carla.Vehicle

The carla.Vehicle. We need this class to spawn our gnss and imu sensor.

config_yamldict

The configuration dictionary of the localization module.

carla_mapcarla.Map

The carla HDMap. We need this to find the map origin to convert wg84 to enu coordinate system.

Attributes gnss : opencda object

GNSS sensor manager for spawning gnss sensor and listen to the data transmission.

ImuSensoropencda object

Imu sensor manager for spawning gnss sensor and listen to the data transmission.

kfopencda object

The filter used to fuse different sensors.

debug_helperopencda object

The debug helper is used to visualize the accuracy of the localization and provide evaluation functions.

add_heading_direction_noise(heading_direction)

Add synthetic noise to heading direction.

Parameters:

heading_direction (float) – groundtruth heading_direction obtained from the server.

Returns:

heading_direction – heading direction with noise.

Return type:

float

add_speed_noise(speed)

Add gaussian white noise to the current speed.

Parameters:

speed (float) – m/s, current speed.

Returns:

speed – the speed with noise.

Return type:

float

destroy()

Destroy the sensors

get_ego_pos()

Retrieve ego vehicle position

get_ego_spd()

Retrieve ego vehicle speed

localize()

Currently implemented in a naive way.

Module contents