2.2.4.4. sensors package

Implementation of all sensors used for the UKF

2.2.4.4.1. Submodules

2.2.4.4.2. sensors.aceinna module

Implementation of all Sensors of the Aceinna IMU.

Implementation of the gyro and accelerometer of the Aceinna IMU.

class sensors.aceinna.AceinnaAcc(filter: ukf.UKF, sensor_name: str, z_names: List[str], z_array: npt.NDArray | None = None, R_array: npt.NDArray | None = None, active: bool = True, R: float = 0.1)[source]

Bases: Sensor

Sensor class for the accelerometer of the Aceinna IMU.

Since the state vector of the chosen filter only tracks the acceleration in x direction, the sensor also only measures the acceleration in x direction. The hx_inverse function does not need to be redefined, since the default function is sufficient.

filter

KalmanFilter the sensor will be used for.

Type:

ukf.UKF

sensor_name

Name to identify the sensor by.

Type:

str

sensor_i

Index of the sensor as part of the Kalman Filter.

Type:

int

z_names

Names of the measurements of the sensor.

Type:

List[str]

z_dim

Dimension of measurement vector derived by length of z_names.

Type:

npt.NDArray

z_array

Numpy array of prerecorded measurements.

Type:

npt.NDArray

R_array

Numpy array of prerecorded measurement noise covariance matrices.

Type:

npt.NDArray

active

Specifies whether the update for a sensor should be executed or only the measurements should be read in.

Type:

bool

x_indices_to_update

Indices of the state variables to use for predict and update. Based on base and situation dependent extended.

Type:

Set

x_indices_to_update_base

Base of indices of the state variables to always use for predict and update.

Type:

Set

z_indices_to_update

Measurement indices to use for update.

Type:

npt.NDArray

z_indices_to_initialize

Measurement indices to initialize while post_processing update.

Type:

npt.NDArray

execute_update_postprocess

Flag to decide whether the update of the sensor should be postprocessed. Used to intiialize intern variables or new state variables.

Type:

bool

R

Numpy array containing the constant measurement noise, by default None

Type:

Optional[npt.NDArray], optional

Initialize an object of the accelerator sensor of the Aceinna IMU.

Parameters:

  • filter (ukf.UKF) – KalmanFilter object the sensor will be used for.

  • sensor_name (str) – Name to identify the sensor by.

  • z_names (List[str]) – List of the names of the measurements with the measurement count as length.

  • z_array (npt.NDArray, optional) – Numpy Array of the prerecorded measurements. Prerecorded measurements of all sensor must be synchronized and have the same length, by default None.

  • R_array (Optional[npt.NDArray], optional) – Numpy array of prerecorded measurement noises, by default None

  • active (bool, optional) – Specifies whether the update for a sensor should be executed or only the measurements should be read in, by default True

  • R (float, optional) – Standard deviation of the longitudinal acceleration in m/s^2, by default 0.1

get_prerecorded_z_and_R(last_prediction_step: int, i: int)[source]

Get prerecorded measurement and measurement noise for current prediction step.

Uses mean of all measurements since last prediction step.

Parameters:

  • last_prediction_step (int) – Last executed prediction step.

  • i (int) – Current index of the prerecorded measurement and measurement noise array.

hx(sigmas_f: ndarray[Any, dtype[floating]], u: ndarray[Any, dtype[ScalarType]] | None) → ndarray[Any, dtype[ScalarType]][source]

Calculate the measurement the sensor should measure based on the current state vector and control input.

Map the tracked acceleration in x direction onto the measurement. No conversion of the unit of the measurement, since both are in [m/s].

Parameters:

  • sigmas_f (npt.NDArray[np.floating]) – Sigma points of the current state vector of the system.

  • u (Optional[npt.NDArray]) – Current control input vector of the system.

Returns:

Calculated measurement vector of the sensor.

Return type:

npt.NDArray

class sensors.aceinna.AceinnaGyro(filter: ukf.UKF, sensor_name: str, z_names: List[str], z_array: npt.NDArray | None = None, R_array: npt.NDArray | None = None, active: bool = True, R: float = 2.0)[source]

Bases: Sensor

Sensor class for the gyrometer of the Aceinna IMU.

Since the state vector of the chosen filter only tracks the angular rate around the z axis, the sensor also only measures the angular rate around the z axis. The hx_inverse function needs to be redefined, since the default function is not sufficient.

filter

KalmanFilter the sensor will be used for.

Type:

ukf.UKF

sensor_name

Name to identify the sensor by.

Type:

str

sensor_i

Index of the sensor as part of the Kalman Filter.

Type:

int

z_names

Names of the measurements of the sensor.

Type:

List[str]

z_dim

Dimension of measurement vector derived by length of z_names.

Type:

npt.NDArray

z_array

Numpy array of prerecorded measurements.

Type:

npt.NDArray

R_array

Numpy array of prerecorded measurement noise covariance matrices.

Type:

npt.NDArray

active

Specifies whether the update for a sensor should be executed or only the measurements should be read in.

Type:

bool

x_indices_to_update

Indices of the state variables to use for predict and update. Based on base and situation dependent extended.

Type:

Set

x_indices_to_update_base

Base of indices of the state variables to always use for predict and update.

Type:

Set

z_indices_to_update

Measurement indices to use for update.

Type:

npt.NDArray

z_indices_to_initialize

Measurement indices to initialize while post_processing update.

Type:

npt.NDArray

execute_update_postprocess

Flag to decide whether the update of the sensor should be postprocessed. Used to intiialize intern variables or new state variables.

Type:

bool

R

Numpy array containing the constant measurement noise, by default None

Type:

Optional[npt.NDArray], optional

Initialize an object of the gyrometer sensor of the Aceinna IMU.

Parameters:

  • filter (ukf.UKF) – KalmanFilter object the sensor will be used for.

  • sensor_name (str) – Name to identify the sensor by.

  • z_names (List[str]) – List of the names of the measurements with the measurement count as length.

  • z_array (npt.NDArray, optional) – Numpy Array of the prerecorded measurements. Prerecorded measurements of all sensor must be synchronized and have the same length, by default None.

  • R_array (Optional[npt.NDArray], optional) – Numpy array of prerecorded measurement noises, by default None

  • active (bool, optional) – Specifies whether the update for a sensor should be executed or only the measurements should be read in, by default True

  • R (float, optional) – Standard deviation of the yaw rate in degrees/s, by default 0.1

get_prerecorded_z_and_R(last_prediction_step: int, i: int)[source]

Get prerecorded measurement and measurement noise for current prediction step.

Uses mean of all measurements since last prediction step.

Parameters:

  • last_prediction_step (int) – Last executed prediction step.

  • i (int) – Current index of the prerecorded measurement and measurement noise array.

hx(sigmas_f: ndarray[Any, dtype[floating]], u: ndarray[Any, dtype[ScalarType]] | None) → ndarray[Any, dtype[ScalarType]][source]

Calculate the measurement the sensor should measure based on the current state vector and control input.

Map the tracked angular rate around the z axis onto the measurement. Convert the unit of the state [rad/s] to the unit of the measurement [degree/s].

Parameters:

  • sigmas_f (npt.NDArray[np.floating]) – Sigma points of the current state vector of the system.

  • u (Optional[npt.NDArray]) – Current control input vector of the system.

Returns:

Calculated measurement vector of the sensor.

Return type:

npt.NDArray

hx_inverse(z: ndarray[Any, dtype[ScalarType]], u: ndarray[Any, dtype[ScalarType]] | None = None) → ndarray[Any, dtype[ScalarType]][source]

Calculate the state vector based on the current measurement of the sensor.

Convert the unit of the measurement [degree/s] to the unit of the state [rad/s].

Parameters:

z (np.ndarray) – Measurement vector, can be a matrix with multiple time steps.

Returns:

State vector, can be a matrix with multiple time steps.

Return type:

np.ndarray

2.2.4.4.3. sensors.landmarks module

Implementation of all Sensors concerning landmarks.

class sensors.landmarks.LocalMapping(filter: ukf.UKF, sensor_name: str, z_names: List[str], z_array: npt.NDArray | None = None, R_array: npt.NDArray | None = None, active: bool = False, data_association: helpers.DataAssociation = DataAssociation.ClusterJCBB, use_kd_tree: bool = True, chi2_quantile: float = 0.8, fov_min_distance: float = 0, fov_max_distance: float = 12, fov_angle: float = 100, fov_scaling: float = 1.2, probability_gate: List[float] = [0.8, 0.8, 0.8, 0.94], R_azimuth: float = 10, R_distance_factor: float = 0.15, R_offset: float = 0.15, update_strategy: LandmarkUpdateStrategy | None = LandmarkUpdateStrategy.ALL)[source]

Bases: Sensor

Sensor class for Landmarks from the local mapping.

Implements the LocalMapping sensor from EVA.

filter

KalmanFilter the sensor will be used for.

Type:

ukf.UKF

sensor_name

Name to identify the sensor by.

Type:

str

sensor_i

Index of the sensor as part of the Kalman Filter.

Type:

int

z_names

Names of the measurements of the sensor.

Type:

List[str]

z_dim

Dimension of measurement vector derived by length of z_names.

Type:

npt.NDArray

z_array

Numpy array of prerecorded measurements.

Type:

npt.NDArray

R_array

Numpy array of prerecorded measurement noise covariance matrices.

Type:

npt.NDArray

active

Specifies whether the update for a sensor should be executed or only the measurements should be read in.

Type:

bool

x_indices_to_update

Indices of the state variables to use for predict and update. Based on base and situation dependent extended.

Type:

Set

x_indices_to_update_base

Base of indices of the state variables to always use for predict and update.

Type:

Set

z_indices_to_update

Measurement indices to use for update.

Type:

npt.NDArray

z_indices_to_initialize

Measurement indices to initialize while post_processing update.

Type:

npt.NDArray

execute_update_postprocess

Flag to decide whether the update of the sensor should be postprocessed. Used to intiialize intern variables or new state variables.

Type:

bool

R_azimut

Measurement noise variance for the azimut angle of the landmark measurement.

Type:

float

R_distance_factor

Factor by which the distance variance increases with distance.

Type:

float

R_offset

Offset of the line used to increase variance with distance.

Type:

float

camera_offset

Offset of the camera position relative to the vehicle origin (middle of the rear axle).

Type:

npt.NDArray

lidar_offset

Offset of the LiDAR position relative to the vehicle origin (middle of the rear axle).

Type:

npt.NDArray

fov_angle

Field of View angle of the camera.

Type:

float

fov_min_distance

Minimum field of view distance of the camera. Measured landmarks will only be considered if they are not closer.

Type:

float

fov_max_distance

Maximum field of view distance of the camera. Measured landmarks will only be considered if they are not farther away.

Type:

float

fov_scaling

Scaling factor for the Field of View angle and distance to extract the theoretically measurable landmarks.

Type:

float

probabilty_gate

Minimum probabilty for which landmarks will be considered. Different for each cone color.

Type:

List[float]

_colors

Mapping of the landmark id/type to the respective color.

Type:

List[str]

_size

Mapping of the landmark id/type to the respective size.

Type:

List[int]

chi2_quantile

Threshold for the mahalanobis distance to associate measured and tracked landmarks.

Type:

float

data_association

Which data association algorithm to use.

Type:

helpers.DataAssociation

use_kd_tree

Whether to use the KD Tree for data association or not.

Type:

bool

Initialize an object of the LocalMapping sensor of autonomous system.

Parameters:

  • filter (ukf.UKF) – KalmanFilter object the sensor will be used for.

  • sensor_name (str) – Name to identify the sensor by.

  • z_names (List[str]) – List of the names of the measurements with the measurement count as length.

  • z_array (npt.NDArray, optional) – Numpy Array of the prerecorded measurements. Prerecorded measurements of all sensor must be synchronized and have the same length, by default None.

  • R_array (Optional[npt.NDArray], optional) – Numpy array of prerecorded measurement noises, by default None

  • active (bool, optional) – Specifies whether the update for a sensor should be executed or only the measurements should be read in, by default True

  • data_association (helpers.DataAssociation, optional) – Which data association algorithm to use, by default helpers.DataAssociation.ClusterJCBB

  • use_kd_tree (bool, optional) – Whether to use the KD Tree for data association or not.

  • chi2_quantile (float, optional) – Threshold for the mahalanobis distance to associate measured and tracked landmarks.

  • fov_min_distance (float, optional) – Minimum field of view distance of the camera. Measured landmarks will only be considered if they are not closer.

  • fov_max_distance (float, optional) – Maximum field of view distance of the camera. Measured landmarks will only be considered if they are not farther away.

  • fov_angle (float, optional) – Field of View angle of the camera.

  • fov_scaling (float, optional) – Scaling factor for the Field of View angle and distance to extract the theoretically measurable landmarks.

  • probability_gate (List, Optional) – Minimum probabilty for which landmarks will be considered. Different for each cone color.

  • R_azimuth (float, optional) – Measurement noise variance for the azimut angle of the landmark measurement.

  • R_distance_factor (float, optional) – Factor by which the distance variance increases with distance.

  • R_offset (float, optional) – Offset of the line used to increase variance with distance.

property R_to_update

Returns measurement noise to update.

Returns:

Measurement noise for the measurements to update.

Return type:

npt.NDArray

calculate_cartesian_cone_covariance(polar_cone: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Computes the covariance matrix in cartesian coordinates for a given cone in polar coordinates. [Hal92]

Parameters:

polar_cone (npt.NDArray) – Coordinates of the cone in polar coordinates.

Returns:

Cone covariance in cartesian coordinates.

Return type:

npt.NDArray

calculate_polar_cone_covariance(polar_cones: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Computes the covariance matrix in polar coordinates for given set of cones in polar coordinates.

Uses a predefined variance for the azimut angle. Uses a percentage of the distance for the standard deviation of the distance.

Parameters:

polar_cones (npt.NDArray) – Coordinates of the cones in polar coordinates.

Returns:

Cones covariances in polar coordinates.

Return type:

npt.NDArray

compute_known_indices(mapping: ndarray[Any, dtype[ScalarType]]) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]][source]

Computes indices of which measured landmarks are known and which are unknown

Parameters:

mapping (npt.NDArray) – Mapping / association between the observed landmarks and all tracked landmarks.

Returns:

Indices of all observed landmarks that are already tracked, Indices of all observed landmarks that are not already tracked.

Return type:

Tuple[npt.NDArray, npt.NDArray]

compute_landmark_mapping(z: ndarray[Any, dtype[ScalarType]], observable_landmarks: ndarray[Any, dtype[ScalarType]], observable_mapping: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Computes the mapping between the currently observed landmarks and all tracked landmarks.

Only uses the currently oberservable landmarks for the calculation. Later translate this mapping into the desired one. Very naive and inconsistent approach. Calculates the mahalanobis distance for every possible combination.

Parameters:

  • z (npt.NDArray) – Current measurement vector of the sensor. [(x, y, type, propability), …].

  • observable_landmarks (npt.NDArray) – Subset of all tracked landmarks that could be observed right now.

  • observable_mapping (npt.NDArray) – Mapping / Association between the observable landmarks and all tracked landmarks.

Returns:

Mapping / Associations between the observed landmarks and all tracked landmarks.

Return type:

npt.NDArray

convert_measurement_to_global_landmarks(z: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Converts cartesian coordinates of local observed coordinates to global cartesian coordinates.

Parameters:

z (npt.NDArray) – Current measurement vector of the sensor. [(x, y, type, probability, is_lidar), …].

Returns:

Global cartesian coordinates of observed landmarks.

Return type:

npt.NDArray

copy(filter: ukf.UKF, timestamp: float) → LocalMapping[source]
static get_color_for_cone_ids(cone_ids: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]
get_prerecorded_z_and_R(last_prediction_step: int, i: int) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]][source]

Just get the current measurement.

Parameters:

  • last_prediction_step (int) – Last executed prediction step.

  • i (int) – Current index of the prerecorded measurement and measurement noise array.

static get_size_for_cone_ids(cone_ids: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]
hx(sigmas_f: ndarray[Any, dtype[floating]], u: ndarray[Any, dtype[floating]] | None) → ndarray[Any, dtype[floating]][source]

Computes the measurement vector the sensor should measure based on the current state vector and camera offset.

Parameters:

  • sigmas_f (npt.NDArray[np.floating]) – Sigma point of the current state vector of the system.

  • u (Optional[npt.NDArray[np.floating]]) – Current control input vector of the system. Unused for this sensor.

Returns:

Measurement state vector that should be measured given the current state vector.

Return type:

npt.NDArray[np.floating]

hx_inverse(z: ndarray, u: ndarray[Any, dtype[ScalarType]] | None = None) → ndarray[Any, dtype[ScalarType]][source]

Calculate the state vector based on the current measurement of the sensor. # TODO: Implement :D .

Not every state can be calculated, so only part of the state vector will be calculated. Use the transformer to convert lat, lon to local coordinates

Parameters:

z (np.ndarray) – Measurement vector, can be a matrix with multiple time steps.

Returns:

State vector, can be a matrix with multiple time steps.

Return type:

np.ndarray

initialize_landmark(global_landmark: ndarray[Any, dtype[ScalarType]], local_coords_polar: ndarray[Any, dtype[ScalarType]]) → None[source]

Initializes new, unknown landmark. Expands state vector x and system noise covariance matrices P and Q.

Parameters:

  • global_landmark (npt.NDArray) – Global position and type of landmark.

  • local_coords_polar (npt.NDArray) – Local position of landmark in polar coordinates.

limit_to_observable_landmarks() → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]][source]

Limits and returns the observable subset of all tracked landmarks based on the current vehicle state and FoV.

Returns:

Position of the landmarks and mapping between the observable landmarks and all tracked landmarks.

Return type:

Tuple[npt.NDArray, npt.NDArray]

static measurement_mean(sigmas: ndarray[Any, dtype[ScalarType]], Wm: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Calculates the weighted mean of multiple measurement vectors.

Parameters:

  • sigmas (npt.NDArray) – Sigma points of the measurement vector.

  • Wm (npt.NDArray) – Weights for the calculation of the weighted mean of the measurement.

Returns:

Weighted mean of the sigma points of the measurement vector.

Return type:

npt.NDArray

postprocess_update() → None[source]

Postprocess update step for this sensor.

Used to intitialize newly observed landmarks as new state variables and save the current observed landmarks.

prepare_update() → None[source]

Prepares update step and returns whether the update for this sensor should be executed.

Doesn’t save measurement and measurement noise for this sensor in the filter object, but calculates measurement indices respectively for update and intiialization and state variable indices for predict and update. This indices are based on the previous executed data association between tracked landmarks (evaporated to obersavable landmarks) and observed landmarks.

Returns:

Specifies wthether the update for this sensor should be executed.

Return type:

bool

static residual_sigmash_and_z(sigmas_h: ndarray[Any, dtype[floating]], z: ndarray[Any, dtype[floating]]) → ndarray[Any, dtype[floating]][source]

Calculates the difference between the measurement sigma points and the measurement.

Parameters:

  • sigmas_h (npt.NDArray[np.floating]) – Sigma points for the measurement.

  • z (npt.NDArray[np.floating]) – Measurement

Returns:

Difference between sigma points and measurements.

Return type:

npt.NDArray[np.floating]

property z_to_update

Returns measurements to update.

Returns:

Measurements to update

Return type:

npt.NDArray

2.2.4.4.4. sensors.novatel module

Implementation of all Sensors of the Novatel GPS.

class sensors.novatel.NovatelGPSHeading(filter: ukf.UKF, sensor_name: str, z_names: List[str], z_array: npt.NDArray | None = None, R_array: npt.NDArray | None = None, active: bool = True, offset: float = 0)[source]

Bases: Sensor

Sensor class for the heading measurement of the novatel gps.

filter

KalmanFilter the sensor will be used for.

Type:

ukf.UKF

sensor_name

Name to identify the sensor by.

Type:

str

sensor_i

Index of the sensor as part of the Kalman Filter.

Type:

int

z_names

Names of the measurements of the sensor.

Type:

List[str]

z_dim

Dimension of measurement vector derived by length of z_names.

Type:

npt.NDArray

z_array

Numpy array of prerecorded measurements.

Type:

npt.NDArray

R_array

Numpy array of prerecorded measurement noise covariance matrices.

Type:

npt.NDArray

active

Specifies whether the update for a sensor should be executed or only the measurements should be read in.

Type:

bool

x_indices_to_update

Indices of the state variables to use for predict and update. Based on base and situation dependent extended.

Type:

Set

x_indices_to_update_base

Base of indices of the state variables to always use for predict and update.

Type:

Set

z_indices_to_update

Measurement indices to use for update.

Type:

npt.NDArray

z_indices_to_initialize

Measurement indices to initialize while post_processing update.

Type:

npt.NDArray

execute_update_postprocess

Flag to decide whether the update of the sensor should be postprocessed. Used to intiialize intern variables or new state variables.

Type:

bool

R

Covariance matrix.

Type:

npt.NDArray

ros_R

Variable to save R while another callback is executed, so R is not overwritten.

Type:

npt.NDArray

offset

Heading offset to correct.

Type:

float

Initialize an object of the position gps sensor of the novatel gps.

Parameters:

  • filter (ukf.UKF) – KalmanFilter object the sensor will be used for.

  • sensor_name (str) – Name to identify the sensor by.

  • z_names (List[str]) – List of the names of the measurements with the measurement count as length.

  • z_array (npt.NDArray, optional) – Numpy Array of the prerecorded measurements. Prerecorded measurements of all sensor must be synchronized and have the same length, by default None.

  • R_array (Optional[npt.NDArray], optional) – Numpy array of prerecorded measurement noises, by default None

  • active (bool, optional) – Specifies whether the update for a sensor should be executed or only the measurements should be read in, by default True

  • offset (float) – Heading offset to correct.

copy(filter: ukf.UKF, timestamp: float) → NovatelGPSHeading[source]
static generate_R_matrix_from_df(df: DataFrame) → ndarray[Any, dtype[ScalarType]][source]

Generate all measurement noise matrices based on the measurements of the dataframe object.

Parameters:

df (pd.DataFrame) – DataFrame object containing the needed data to calculate the measurement noise matrices.

Returns:

Measurement noise matrices for all measurements.

Return type:

npt.NDArray

get_prerecorded_z_and_R(last_prediction_step: int, i: int)[source]

Get prerecorded measurement and measurement noise for current prediction step.

Using the last valid emasurement since last the last prediction step.

Parameters:

  • last_prediction_step (int) – Last executed prediction step.

  • i (int) – Current index of the prerecorded measurement and measurement noise array.

hx(sigmas_f: ndarray[Any, dtype[floating]], u: ndarray[Any, dtype[float32]] | None) → ndarray[Any, dtype[float32]][source]

Calculate the measurement the sensor should measure based on the current state vector and control input.

Parameters:

  • sigmas_f (npt.NDArray[np.floating]) – Sigma point of the current state vector of the system.

  • u (Optional[npt.NDArray]) – Current control input vector of the system.

Returns:

Calculated measurement vector of the sensor.

Return type:

npt.NDArray

hx_inverse(z: ndarray, u: ndarray[Any, dtype[ScalarType]] | None = None) → ndarray[Any, dtype[ScalarType]][source]

Calculate the state vector based on the current measurement of the sensor.

Not every state can be calculated, so only part of the state vector will be calculated. Use the transformer to convert lat, lon to local coordinates

Parameters:

z (np.ndarray) – Measurement vector, can be a matrix with multiple time steps.

Returns:

State vector, can be a matrix with multiple time steps.

Return type:

npt.NDArray

static measurement_mean(sigmas: ndarray[Any, dtype[ScalarType]], Wm: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Calculates the weighted mean of multiple measurement vectors.

Parameters:

  • sigmas (npt.NDArray) – Sigma points of the measurement vector.

  • Wm (npt.NDArray) – Weights for the calculation of the weighted mean of the measurement.

Returns:

Weighted mean of the sigma points of the measurement vector.

Return type:

npt.NDArray

postprocess_update() → None[source]

Postprocess update step for this sensor by initialzing initial world and map heading if not initialzed yet.

prepare_update() → bool[source]

Prepares update step and returns whether the update for this sensor should be executed.

Saves measurement and measurement noise for this sensor in the filter object and calculates measurement indices respectively for update and intiialization and state variable indices for predict and update. If initial world and map heading have not been set yet and measurements are available, specify to postprocess.

Returns:

Specifies wthether the update for this sensor should be executed.

Return type:

bool

static residual_sigmash_and_z(sigmas_h: ndarray[Any, dtype[floating]], z: ndarray[Any, dtype[floating]]) → ndarray[Any, dtype[floating]][source]

Calculates the difference between the measurement sigma points and the measurement.

Parameters:

  • sigmas_h (npt.NDArray[np.floating]) – Sigma points for the measurement.

  • z (npt.NDArray[np.floating]) – Measurement

Returns:

Difference between sigma points and measurements.

Return type:

npt.NDArray[np.floating]

class sensors.novatel.NovatelGPSPosition(filter: ukf.UKF, sensor_name: str, z_names: List[str], z_array: npt.NDArray | None = None, R_array: npt.NDArray | None = None, active: bool = True)[source]

Bases: Sensor

Sensor class for the position and speed measurement of the novatel gps.

filter

KalmanFilter the sensor will be used for.

Type:

ukf.UKF

sensor_name

Name to identify the sensor by.

Type:

str

sensor_i

Index of the sensor as part of the Kalman Filter.

Type:

int

z_names

Names of the measurements of the sensor.

Type:

List[str]

z_dim

Dimension of measurement vector derived by length of z_names.

Type:

npt.NDArray

z_array

Numpy array of prerecorded measurements.

Type:

npt.NDArray

R_array

Numpy array of prerecorded measurement noise covariance matrices.

Type:

npt.NDArray

active

Specifies whether the update for a sensor should be executed or only the measurements should be read in.

Type:

bool

x_indices_to_update

Indices of the state variables to use for predict and update. Based on base and situation dependent extended.

Type:

Set

x_indices_to_update_base

Base of indices of the state variables to always use for predict and update.

Type:

Set

z_indices_to_update

Measurement indices to use for update.

Type:

npt.NDArray

z_indices_to_initialize

Measurement indices to initialize while post_processing update.

Type:

npt.NDArray

execute_update_postprocess

Flag to decide whether the update of the sensor should be postprocessed. Used to intiialize intern variables or new state variables.

Type:

bool

_transformer

Transformer to convert gps measurements in world frame to map frame.

Type:

pyproj.Transformer

R

Covariance matrix.

Type:

npt.NDArray

ros_R

Variable to save R while another callback is executed, so R is not overwritten.

Type:

npt.NDArray

Initialize an object of the position gps sensor of the novatel gps.

Parameters:

  • filter (ukf.UKF) – KalmanFilter object the sensor will be used for.

  • sensor_name (str) – Name to identify the sensor by.

  • z_names (List[str]) – List of the names of the measurements with the measurement count as length.

  • z_array (npt.NDArray, optional) – Numpy Array of the prerecorded measurements. Prerecorded measurements of all sensor must be synchronized and have the same length, by default None.

  • R_array (Optional[npt.NDArray], optional) – Numpy array of prerecorded measurement noises, by default None

  • active (bool, optional) – Specifies whether the update for a sensor should be executed or only the measurements should be read in, by default True

copy(filter: ukf.UKF, timestamp: float) → NovatelGPSPosition[source]
create_transformer(lat_0: float = 0, lon_0: float = 0, alpha: float = 0, x_0=0, y_0=0) → Transformer[source]

Create a transformer object once to transform global and local coordinates.

Initialize transformer with current global measurement, current estimated local position and global heading.

Parameters:

  • lat_0 (float) – global origin of the latitude of the transformer object

  • lon_0 (float) – global origin of the longitude of the transformer object

  • alpha (float) – Azimuth of centerline clockwise from north at the center point of the line

  • x_0 (int, optional) – local origin of the transformer object, by default 0

  • y_0 (int, optional) – local origin of the transformer object, by default 0

Returns:

Transformer to convert gps measurements in world frame to map frame.

Return type:

pyproj.Transformer

static generate_R_matrix_from_df(df: DataFrame) → ndarray[Any, dtype[ScalarType]][source]

Generate all measurement noise matrices based on the measurements of the dataframe object.

Parameters:

df (pd.DataFrame) – DataFrame object containing the needed data to calculate the measurement noise matrices.

Returns:

Measurement noise matrices for all measurements.

Return type:

npt.NDArray

get_prerecorded_z_and_R(last_prediction_step: int, i: int)[source]

Get prerecorded measurement and measurement noise for current prediction step.

Using the last valid emasurement since last the last prediction step.

Parameters:

  • last_prediction_step (int) – Last executed prediction step.

  • i (int) – Current index of the prerecorded measurement and measurement noise array.

static hx(sigmas_f: ndarray[Any, dtype[floating]], u: ndarray[Any, dtype[float32]] | None) → ndarray[Any, dtype[float32]][source]

Calculate the measurement the sensor should measure based on the current state vector and control input.

Parameters:

  • sigmas_f (npt.NDArray[np.floating]) – Sigma points of the current state vector of the system.

  • u (Optional[npt.NDArray]) – Current control input vector of the system.

Returns:

Calculated measurement vector of the sensor.

Return type:

npt.NDArray

hx_inverse(z: ndarray[Any, dtype[ScalarType]], u: ndarray[Any, dtype[ScalarType]] | None = None) → ndarray[Any, dtype[ScalarType]][source]

Calculate the state vector based on the current measurement of the sensor.

Not every state can be calculated, so only part of the state vector will be calculated. Use the transformer to convert lat, lon to local coordinates

Parameters:

z (np.ndarray) – Measurement vector, can be a matrix with multiple time steps.

Returns:

State vector, can be a matrix with multiple time steps.

Return type:

npt.NDArray

postprocess(saver: Saver)[source]

Postprocess measurements and measurement noises of sensor.

Transform position measurements in latitude in world frame to to position measurements in map frame.

Parameters:

saver (helpers.Saver) – Saver object containing the data of the novatel gps position sensor measurements to be postprocessed.

postprocess_update() → None[source]

Postprocess update step for this sensor by initialzing transformer object if initial map and world heading have been intialized already.

prepare_update() → bool[source]

Prepares update step and returns whether the update for this sensor should be executed.

Saves measurement and measurement noise for this sensor in the filter object and calculates measurement indices respectively for update and intiialization and state variable indices for predict and update. If initial world and map heading are initialized, but the transformer not yet and measurements are available, specify to postprocess to initialize the transformer object. If everything has been initialized already, transform gps measurement to map frame and specifiy to update.

Returns:

Specifies wthether the update for this sensor should be executed.

Return type:

bool

set_z_and_R_from_buffer(timestamp: float)[source]

2.2.4.4.5. sensors.sensor module

Implementation of the baseline sensor class.

class sensors.sensor.Sensor(filter: ukf.UKF, sensor_name: str, z_names: List[str], z_array: npt.NDArray | None = None, R_array: npt.NDArray | None = None, active: bool = True)[source]

Bases: object

Baseline sensor class.

This class is used to extend the KalmanFilter class to handle multiple measurements dynamically.

filter

KalmanFilter the sensor will be used for.

Type:

ukf.UKF

sensor_name

Name to identify the sensor by.

Type:

str

sensor_i

Index of the sensor as part of the Kalman Filter.

Type:

int

z_names

Names of the measurements of the sensor.

Type:

List[str]

z_dim

Dimension of measurement vector derived by length of z_names.

Type:

npt.NDArray

z_array

Numpy array of prerecorded measurements.

Type:

npt.NDArray

R_array

Numpy array of prerecorded measurement noise covariance matrices.

Type:

npt.NDArray

active

Specifies whether the update for a sensor should be executed or only the measurements should be read in.

Type:

bool

x_indices_to_update

Indices of the state variables to use for predict and update. Based on base and situation dependent extended.

Type:

Set

x_indices_to_update_base

Base of indices of the state variables to always use for predict and update.

Type:

Set

z_indices_to_update

Measurement indices to use for update.

Type:

npt.NDArray

z_indices_to_initialize

Measurement indices to initialize while post_processing update.

Type:

npt.NDArray

execute_update_postprocess

Flag to decide whether the update of the sensor should be postprocessed. Used to intiialize intern variables or new state variables.

Type:

bool

z

Measurement.

Type:

npt.NDArray

ros_z

Variable to save z while another callback is executed, so z is not overwritten.

Type:

npt.NDArray

ros_t_array

List to store timestamps of received sensor measurements.

Type:

List[float]

ros_z_array

List to store received sensor measurements corresponding to the timestamps in ros_t_array.

Type:

List[npt.NDArray]

ros_R_array

List to store the covariance matrices of the sensor measurements in ros_z_array at times in ros_t_array.

Type:

List[npt.NDArray]

Initializes a Sensor object.

Parameters:

  • filter (ukf.UKF) – KalmanFilter object the sensor will be used for.

  • sensor_name (str) – Name to identify the sensor by.

  • z_names (List[str]) – List of the names of the measurements with the measurement count as length.

  • z_array (npt.NDArray, optional) – Numpy Array of the prerecorded measurements. Prerecorded measurements of all sensor must be synchronized and have the same length, by default None.

  • R_array (Optional[npt.NDArray], optional) – Numpy array of prerecorded measurement noises, by default None

  • active (bool, optional) – Specifies whether the update for a sensor should be executed or only the measurements should be read in, by default True

property R_to_update

Returns measurement noise to update.

Returns:

Measurement noise for the measurements to update.

Return type:

npt.NDArray

get_prerecorded_z_and_R(last_prediction_step: int, i: int)[source]

Get prerecorded measurement and measurement noise for current prediction step.

Parameters:

  • last_prediction_step (int) – Last executed prediction step.

  • i (int) – Current index of the prerecorded measurement and measurement noise array.

hx(sigmas_f: ndarray[Any, dtype[floating]], u: ndarray[Any, dtype[ScalarType]] | None) → ndarray[Any, dtype[ScalarType]][source]

Calculate the measurement the sensor should measure based on the current state vector and control input.

Dummy function which only returns the current state vector. The detailed sensors will implement this function for the according context.

Parameters:

  • sigmas_f (npt.NDArray[npt.NDArray]) – Sigma points of the current state vector of the system.

  • u (Optional[npt.NDArray]) – Current control input vector of the system.

Returns:

Calculated measurement vector of the sensor.

Return type:

npt.NDArray

hx_inverse(z: ndarray[Any, dtype[ScalarType]], u: ndarray[Any, dtype[ScalarType]] | None = None) → ndarray[Any, dtype[ScalarType]][source]

Calculate the state vector based on the current measurement of the sensor.

Not every state can be calculated, so only part of the state vector will be calculated.

Parameters:

z (np.ndarray) – Measurement vector, can be a matrix with multiple time steps.

Returns:

State vector, can be a matrix with multiple time steps.

Return type:

np.ndarray

static measurement_mean(sigmas: ndarray[Any, dtype[ScalarType]], Wm: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Calculates the weighted mean of multiple measurement vectors.

Parameters:

  • sigmas (npt.NDArray) – Sigma points of the measurement vector.

  • Wm (npt.NDArray) – Weights for the calculation of the weighted mean of the measurement.

Returns:

Weighted mean of the sigma points of the measurement vector.

Return type:

npt.NDArray

pop_z_and_R_from_buffer(local_mapping_timestamp: float)[source]
postprocess(saver: Saver)[source]

Postprocess measurements and measurement noises of sensor.

Parameters:

saver (helpers.Saver) – Saver object containing the data to postprocess.

postprocess_update() → None[source]

Postprocess update step for this sensor.

Used to intitialize intern variable or a new state variable.

prepare_update() → bool[source]

Prepares update step and returns whether the update for this sensor should be executed.

Saves measurement and measurement noise for this sensor in the filter object and calculates measurement indices respectively for update and intiialization and state variable indices for predict and update.

Returns:

Specifies wthether the update for this sensor should be executed.

Return type:

bool

static residual_sigmash_and_z(sigmas_h: ndarray[Any, dtype[floating]], z: ndarray[Any, dtype[floating]]) → ndarray[Any, dtype[floating]][source]

Calculates the difference between the measurement sigma points and the measurement.

Parameters:

  • sigmas_h (npt.NDArray[np.floating]) – Sigma points for the measurement.

  • z (npt.NDArray[np.floating]) – Measurement.

Returns:

Difference between sigma points and measurements.

Return type:

npt.NDArray[np.floating]

set_z_and_R_from_buffer(timestamp: float)[source]
property z_to_update: ndarray[Any, dtype[ScalarType]]

Returns measurements to update.

Returns:

Measurements to update

Return type:

npt.NDArray

2.2.4.4.6. sensors.wheelspeed module

Implementation of all wheelspeed sensors.

class sensors.wheelspeed.CUREDiffWheelspeed(filter: ukf.UKF, sensor_name: str, z_names: List[str], z_array: npt.NDArray | None = None, R_array: npt.NDArray | None = None, active: bool = True, right: bool = True, gear_ratio: float = 1, wheel_radius: float = 0.235, track_width: float = 1.18112, R: float = 50.0)[source]

Bases: Sensor

Sensor class for a differential wheelspeed.

Differential means that the measurement function calculates the wheelspeed based on the vehicle speed and ackermann angle.

filter

KalmanFilter the sensor will be used for.

Type:

ukf.UKF

sensor_name

Name to identify the sensor by.

Type:

str

sensor_i

Index of the sensor as part of the Kalman Filter.

Type:

int

z_names

Names of the measurements of the sensor.

Type:

List[str]

z_dim

Dimension of measurement vector derived by length of z_names.

Type:

npt.NDArray

z_array

Numpy array of prerecorded measurements.

Type:

npt.NDArray

R_array

Numpy array of prerecorded measurement noise covariance matrices.

Type:

npt.NDArray

active

Specifies whether the update for a sensor should be executed or only the measurements should be read in.

Type:

bool

x_indices_to_update

Indices of the state variables to use for predict and update. Based on base and situation dependent extended.

Type:

Set

x_indices_to_update_base

Base of indices of the state variables to always use for predict and update.

Type:

Set

z_indices_to_update

Measurement indices to use for update.

Type:

npt.NDArray

z_indices_to_initialize

Measurement indices to initialize while post_processing update.

Type:

npt.NDArray

execute_update_postprocess

Flag to decide whether the update of the sensor should be postprocessed. Used to intiialize intern variables or new state variables.

Type:

bool

R

Numpy array containing the constant measurement noise, by default None

Type:

Optional[npt.NDArray], optional

right

Attribute to specify whether the wheel is on the left or right.

Type:

bool

gear_ratio

Gear ratio of the wheelspeed sensor and wheel.

Type:

float

wheel_radius

Wheel radius.

Type:

float

track_width

Track width of the corresponding axle.

Type:

float

Initialize an object of a differential wheelspeed sensor.

Parameters:

  • filter (ukf.UKF) – KalmanFilter object the sensor will be used for.

  • sensor_name (str) – Name to identify the sensor by.

  • z_names (List[str]) – List of the names of the measurements with the measurement count as length.

  • z_array (npt.NDArray, optional) – Numpy Array of the prerecorded measurements. Prerecorded measurements of all sensor must be synchronized and have the same length, by default None

  • R_array (Optional[npt.NDArray], optional) – Numpy array of prerecorded measurement noises, by default None

  • active (bool, optional) – Specifies whether the update for a sensor should be executed or only the measurements should be read in, by default True

  • right (bool, optional) – Attribute to specify whether the wheel is on the left or right, by default True

  • gear_ratio (float, optional) – Gear ratio of the wheelspeed sensor and wheel, by default 1

  • wheel_radius (float, optional) – Radius of the wheel, by default 0.235

  • track_width (float, optional) – Track width of the corresponding axle, by default 1.18112

  • R (float, optional) – Standard deviation of the wheelspeed in rpm, by default 50.

copy(filter: ukf.UKF, timestamp: float) → CUREDiffWheelspeed[source]
get_prerecorded_z_and_R(last_prediction_step: int, i: int)[source]

Get prerecorded measurement and measurement noise for current prediction step.

Uses mean of all measurements since last prediction step.

Parameters:

  • last_prediction_step (int) – Last executed prediction step.

  • i (int) – Current index of the prerecorded measurement and measurement noise array.

hx(sigmas_f: ndarray[Any, dtype[floating]], u: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Calculate the measurement the sensor should measure based on the current state vector and control input.

First, the vehicle speed is calculated mapped onto the left or right track of the vehicle. For this calculation the track_width and current steering angle is used. Second, the according track velocity is casted to a wheelspeed.

Parameters:

  • sigmas_f (npt.NDArray) – Sigma points of the current state vector of the system.

  • u (Optional[npt.NDArray]) – Current control input vector of the system.

Returns:

Calculated measurement vector of the sensor.

Return type:

npt.NDArray

hx_inverse(z: ndarray[Any, dtype[ScalarType]], u: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Calculate the state vector based on the current measurement of the sensor.

Not every state can be calculated, so only part of the state vector will be calculated. At the moment the wheelspeed is directly converted to a track velocity. In the future the ackermann angle and the side of the wheel should be considered.

Parameters:

  • z (npt.NDArray) – Measurement vector, can be a matrix with multiple time steps.

  • u (npt.NDArray) – Current control input vector of the system, by default None

Returns:

State vector, can be a matrix with multiple time steps.

Return type:

npt.NDArray