"""Implementation of all Sensors of the Aceinna IMU.Implementation of the gyro and accelerometer of the Aceinna IMU."""from__future__importannotationsfromtypingimportTYPE_CHECKINGifTYPE_CHECKING:importukffromtypingimportList,Optionalimportnumpy.typingasnptimportnumpyasnpfrom.sensorimportSensor
[docs]classAceinnaAcc(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. 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 """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,R:float=0.1)->None:""" 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 """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])
[docs]defhx(self,sigmas_f:npt.NDArray[np.floating],u:Optional[npt.NDArray])->npt.NDArray:""" 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 ------- npt.NDArray Calculated measurement vector of the sensor. """H_imu=np.array([[0,0,1,0]])returnnp.dot(H_imu,sigmas_f[0:6])
[docs]defget_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)
[docs]classAceinnaGyro(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. 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 """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,R:float=2.)->None:""" 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 """super().__init__(filter,z_array=z_array,R_array=R_array,sensor_name=sensor_name,z_names=z_names)self.R=np.diag([R**2])
[docs]defhx(self,sigmas_f:npt.NDArray[np.floating],u:Optional[npt.NDArray])->npt.NDArray:""" 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 ------- npt.NDArray Calculated measurement vector of the sensor. """H_imu=np.array([[0,0,0,180/np.pi]])returnnp.dot(H_imu,sigmas_f[0:6])
[docs]defhx_inverse(self,z:npt.NDArray,u:Optional[npt.NDArray]=None)->npt.NDArray:""" 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 ------- np.ndarray State vector, can be a matrix with multiple time steps. """returnz*np.pi/180.# type: ignore
[docs]defget_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)