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.