"""
Implementation of all Sensors of the Aceinna IMU.
Implementation of the gyro and accelerometer of the Aceinna IMU.
"""
from __future__ import annotations
from typing import TYPE_CHECKING
if TYPE_CHECKING:
import ukf
from typing import List, Optional
import numpy.typing as npt
import numpy as np
from .sensor import Sensor
[docs]class AceinnaAcc(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] def hx(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]])
return np.dot(H_imu, sigmas_f[0:6])
[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)
[docs]class AceinnaGyro(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] def hx(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]])
return np.dot(H_imu, sigmas_f[0:6])
[docs] def hx_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.
"""
return z * np.pi / 180. # type: ignore
[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)