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.

-configdict

The configuration dictionary of the localization module.

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.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 with noise.

Return type

-heading_direction (float)

add_speed_noise(speed)

Add gaussian white noise to the current speed.

Parameters

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

Returns

the speed with noise.

Return type

-speed (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