opencda.customize.core.sensing.localization package

Submodules

opencda.customize.core.sensing.localization.extented_kalman_filter module

Use Extended Kalman Filter on GPS + IMU for better localization.

class opencda.customize.core.sensing.localization.extented_kalman_filter.ExtentedKalmanFilter(dt)

Bases: object

Extended Kalman Filter implementation for gps and imu.

Parameters -dt : float

The step time for kalman filter calculation.

Attributes -Q : numpy.array

predict state covariance.

-Rnumpy.array

Observation x,y position covariance.

-time_stepfloat

The step time for kalman filter calculation.

-xEstnumpy.array

Estimated x values.

-PEstnumpy.array

The estimated P values.

jacob_f(x, u)

Jacobian of Motion Model motion model

Parameters

-x (np.array) – Input X array.

Returns

Jacobian of Motion Model motion model.

Return type

-jF (np.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

predicted state.

Return type

x (np.array)

observation_model(x)

Project the state.array to sensor measurement.array.

Parameters

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

Returns

predicted measurement.

Return type

-z (np.array)

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

Apply EKF 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

The corrected x, y, heading,

and velocity information.

Return type

  • xEST (np.array)

run_step_init(x, y, heading, velocity)

Initalization for states.

Parameters

  • -x (float) – The X coordinate.

  • -y (float) – Tehe y coordinate.

  • -heading (float) – The heading direction.

  • -velocity (float) – The velocity.

opencda.customize.core.sensing.localization.localization_manager module

Customized Localization Module.

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

Bases: opencda.core.sensing.localization.localization_manager.LocalizationManager

Customized Localization module to replace the default module.

Parameters -vehicle : carla.Vehicle

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

-config_yaml: dict

The configuration dictionary of the localization module.

-carla_map: carla.Map

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

Attributes -kf : opencda object

The filter used to fuse different sensors.

Module contents