"""
Implementation of all wheelspeed sensors.
"""
from __future__ import annotations
from typing import TYPE_CHECKING, Optional, List
if TYPE_CHECKING:
import ukf
import numpy.typing as npt
import numpy as np
from utils import vehicle_dynamics
from .sensor import Sensor
[docs]class CUREDiffWheelspeed(Sensor):
"""
Sensor class for a differential wheelspeed.
Differential means that the measurement function calculates
the wheelspeed based on the vehicle speed and ackermann angle.
Attributes
----------
filter : ukf.UKF
KalmanFilter the sensor will be used for.
sensor_name : str
Name to identify the sensor by.
sensor_i : int
Index of the sensor as part of the Kalman Filter.
z_names : List[str]
Names of the measurements of the sensor.
z_dim : npt.NDArray
Dimension of measurement vector derived by length of z_names.
z_array : npt.NDArray
Numpy array of prerecorded measurements.
R_array : npt.NDArray
Numpy array of prerecorded measurement noise covariance matrices.
active : bool
Specifies whether the update for a sensor should be executed or only the measurements should be read in.
x_indices_to_update : Set
Indices of the state variables to use for predict and update. Based on base and situation dependent extended.
x_indices_to_update_base : Set
Base of indices of the state variables to always use for predict and update.
z_indices_to_update : npt.NDArray
Measurement indices to use for update.
z_indices_to_initialize : npt.NDArray
Measurement indices to initialize while post_processing update.
execute_update_postprocess : bool
Flag to decide whether the update of the sensor should be postprocessed. Used to intiialize intern variables
or new state variables.
R : Optional[npt.NDArray], optional
Numpy array containing the constant measurement noise, by default None
right : bool
Attribute to specify whether the wheel is on the left or right.
gear_ratio : float
Gear ratio of the wheelspeed sensor and wheel.
wheel_radius : float
Wheel radius.
track_width : float
Track width of the corresponding axle.
"""
def __init__(self, filter: ukf.UKF, sensor_name: str, z_names: List[str],
z_array: Optional[npt.NDArray] = None, R_array: Optional[npt.NDArray] = None,
active: bool = True, right: bool = True, gear_ratio: float = 1, wheel_radius: float = 0.235,
track_width: float = 1.18112, R: float = 50.) -> None:
"""
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.
"""
super().__init__(filter, z_array=z_array, R_array=R_array,
sensor_name=sensor_name, z_names=z_names, active=active)
self.R = np.diag([R**2])
self.right = right
self.gear_ratio = gear_ratio
self.wheel_radius = wheel_radius
self.track_width = track_width
[docs] def copy(self, filter: ukf.UKF, timestamp: float) -> 'CUREDiffWheelspeed':
copy = CUREDiffWheelspeed(filter=filter, sensor_name=self.sensor_name, z_names=self.z_names,
active=self.active, right=self.right, gear_ratio=self.gear_ratio,
wheel_radius=self.wheel_radius, track_width=self.track_width)
copy.R = self.R
copy.ros_z_array = self.ros_z_array
copy.ros_t_array = self.ros_t_array
copy.measurement_lock = self.measurement_lock
return copy
[docs] def hx(self, sigmas_f: npt.NDArray[np.floating], u: npt.NDArray) -> npt.NDArray:
"""
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
-------
npt.NDArray
Calculated measurement vector of the sensor.
"""
steering_wheel_angle = u[0] # positive: left turn
ackermann = self.filter.steering_wheel_to_ackermann_lut(steering_wheel_angle)
v_outer = sigmas_f[:, 2].copy()
sign = -1 if self.right else 1
H_wheelspeed = ((v_outer - sign * np.cos(ackermann) * np.radians(u[2]) * (self.track_width / 2))
* ((60 * self.gear_ratio) / (2 * np.pi * self.wheel_radius)))
return np.atleast_2d(H_wheelspeed).T
[docs] def hx_inverse(self, z: npt.NDArray, u: npt.NDArray) -> npt.NDArray:
"""
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
-------
npt.NDArray
State vector, can be a matrix with multiple time steps.
"""
if z.ndim > 1:
z = z[:, 0]
v = np.empty_like(z)
steering_wheel_angle = u[:, 0] # positive: left turn
ackermann = vehicle_dynamics.steering_wheel_to_ackermann(steering_wheel_angle)
v_outer = 2 * z * np.pi * self.wheel_radius / (60 * self.gear_ratio)
condition = (ackermann != 0)
turning_radius = vehicle_dynamics.ackermann_to_radius(ackermann[condition])
sign = 1 if self.right else -1
v[condition] *= (1 + sign * (self.track_width / (2 * turning_radius)))
v[~condition] = v_outer[~condition]
return v[:, np.newaxis]
[docs] def get_prerecorded_z_and_R(self, last_prediction_step: int, i: int):
"""
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.
"""
self.z = np.atleast_1d(np.nanmean(self.z_array[last_prediction_step:i], axis=0))
self.R = np.atleast_1d(self.R)