Skip to content

Sensor Models

commonroad_control.simulation.sensor_models.sensor_model_interface

SensorModelInterface

Bases: ABC

Interface for sensor models which take the current state and control input as an input argument and simulate a (noisy) measurement.

Source code in commonroad_control/simulation/sensor_models/sensor_model_interface.py 
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
class SensorModelInterface(ABC):
    """
    Interface for sensor models which take the current state and control input as an input argument and simulate a (noisy) measurement.
    """

    def __init__(
        self,
        noise_model: UncertaintyModelInterface,
        state_output_factory: Union[StateInputDisturbanceTrajectoryFactoryInterface, Any],
        dim: int,
        state_dimension: int,
        input_dimension: int,
    ):
        """
        Initialize baseclass.
        :param noise_model: uncertainty model representing sensor noise - UncertaintyModelInterface
        :param state_output_factory: factory for creating States or Outputs as output arguments - StateInputDisturbanceTrajectoryFactoryInterface
        :param dim: dimension of the output - int
        :param state_dimension: state dimension - int
        :param input_dimension: input dimension - int
        """
        self._noise_model: UncertaintyModelInterface = noise_model
        self._state_output_factory: Union[StateInputDisturbanceTrajectoryFactoryInterface, Any] = state_output_factory
        self._dim: int = dim
        self._state_dimension: int = state_dimension
        self._input_dimension: int = input_dimension

        # setup casadi function wrapping the nominal output function
        xk = cas.SX.sym("xk", state_output_factory.state_dimension, 1)
        uk = cas.SX.sym("uk", state_output_factory.input_dimension, 1)
        self._output_function = cas.Function("output_function", [xk, uk], [self._output_function_cas(xk, uk)])

    @property
    def noise_model(self) -> UncertaintyModelInterface:
        """
        :return: Uncertainty model representing sensor noise
        """
        return self._noise_model

    @property
    def output_dimension(self) -> int:
        """
        :return: output dimension
        """
        return self._dim

    @property
    def state_dimension(self) -> int:
        """
        :return: state dimension
        """
        return self._state_dimension

    @property
    def input_dimension(self) -> int:
        """
        :return: input dimension
        """
        return self._input_dimension

    @abstractmethod
    def _output_function_cas(
        self, x: Union[np.array, cas.SX.sym], u: Union[np.array, cas.SX.sym]
    ) -> Union[cas.SX.sym, np.array]:
        """
        Implements the nominal output function y=h(x,u), where e.g., h(x,u) = C*x + D*u for linear systems.
        :param x: state
        :param u: control input
        :return: value of the output function evaluated at x, u
        """
        pass

    def nominal_output(self, x: Union[StateInterface, np.array], u: Union[StateInterface, np.array]) -> np.ndarray:
        """
        Evaluates the nominal output value given a state and corresponding control input.
        :param x: state
        :param u: control input
        :return: nominal output at (x,u) represented as a numpy array
        """

        # convert state and input to arrays
        if isinstance(x, StateInterface):
            x_np = x.convert_to_array()
        else:
            x_np = x

        if isinstance(u, InputInterface):
            u_np = u.convert_to_array()
        else:
            u_np = u

        # evaluate output function
        x_next = self._output_function(x_np, u_np).full()

        return x_next.squeeze()

    @abstractmethod
    def measure(self, x: StateInterface, u: InputInterface, rand_noise: bool = True) -> Union[StateInterface, Any]:
        """
        Evaluates the output function and applies (random) noise to the output.
        :param x: state
        :param u: input
        :param rand_noise: true, if random noise shall be applied, otherwise the nominal value of the uncertainty model is applied.
        :return: (noisy) measurement
        """
        pass
input_dimension property

Returns:

Type Description
int

input dimension
noise_model property

Returns:

Type Description
UncertaintyModelInterface

Uncertainty model representing sensor noise
output_dimension property

Returns:

Type Description
int

output dimension
state_dimension property

Returns:

Type Description
int

state dimension
__init__(noise_model, state_output_factory, dim, state_dimension, input_dimension)

Initialize baseclass.

Parameters:

Name Type Description Default
noise_model UncertaintyModelInterface

uncertainty model representing sensor noise - UncertaintyModelInterface

 required
state_output_factory Union[StateInputDisturbanceTrajectoryFactoryInterface, Any]

factory for creating States or Outputs as output arguments - StateInputDisturbanceTrajectoryFactoryInterface

 required
dim int

dimension of the output - int

 required
state_dimension int

state dimension - int

 required
input_dimension int

input dimension - int

 required
Source code in commonroad_control/simulation/sensor_models/sensor_model_interface.py 
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
def __init__(
    self,
    noise_model: UncertaintyModelInterface,
    state_output_factory: Union[StateInputDisturbanceTrajectoryFactoryInterface, Any],
    dim: int,
    state_dimension: int,
    input_dimension: int,
):
    """
    Initialize baseclass.
    :param noise_model: uncertainty model representing sensor noise - UncertaintyModelInterface
    :param state_output_factory: factory for creating States or Outputs as output arguments - StateInputDisturbanceTrajectoryFactoryInterface
    :param dim: dimension of the output - int
    :param state_dimension: state dimension - int
    :param input_dimension: input dimension - int
    """
    self._noise_model: UncertaintyModelInterface = noise_model
    self._state_output_factory: Union[StateInputDisturbanceTrajectoryFactoryInterface, Any] = state_output_factory
    self._dim: int = dim
    self._state_dimension: int = state_dimension
    self._input_dimension: int = input_dimension

    # setup casadi function wrapping the nominal output function
    xk = cas.SX.sym("xk", state_output_factory.state_dimension, 1)
    uk = cas.SX.sym("uk", state_output_factory.input_dimension, 1)
    self._output_function = cas.Function("output_function", [xk, uk], [self._output_function_cas(xk, uk)])
_output_function_cas(x, u) abstractmethod

Implements the nominal output function y=h(x,u), where e.g., h(x,u) = Cx + Du for linear systems.

Parameters:

Name Type Description Default
x Union[array, sym]

state

 required
u Union[array, sym]

control input

 required

Returns:

Type Description
Union[sym, array]

value of the output function evaluated at x, u
Source code in commonroad_control/simulation/sensor_models/sensor_model_interface.py 
77
78
79
80
81
82
83
84
85
86
87
@abstractmethod
def _output_function_cas(
    self, x: Union[np.array, cas.SX.sym], u: Union[np.array, cas.SX.sym]
) -> Union[cas.SX.sym, np.array]:
    """
    Implements the nominal output function y=h(x,u), where e.g., h(x,u) = C*x + D*u for linear systems.
    :param x: state
    :param u: control input
    :return: value of the output function evaluated at x, u
    """
    pass
measure(x, u, rand_noise=True) abstractmethod

Evaluates the output function and applies (random) noise to the output.

Parameters:

Name Type Description Default
x StateInterface

state

 required
u InputInterface

input

 required
rand_noise bool

true, if random noise shall be applied, otherwise the nominal value of the uncertainty model is applied.

 True

Returns:

Type Description
Union[StateInterface, Any]

(noisy) measurement
Source code in commonroad_control/simulation/sensor_models/sensor_model_interface.py 
113
114
115
116
117
118
119
120
121
122
@abstractmethod
def measure(self, x: StateInterface, u: InputInterface, rand_noise: bool = True) -> Union[StateInterface, Any]:
    """
    Evaluates the output function and applies (random) noise to the output.
    :param x: state
    :param u: input
    :param rand_noise: true, if random noise shall be applied, otherwise the nominal value of the uncertainty model is applied.
    :return: (noisy) measurement
    """
    pass
nominal_output(x, u)

Evaluates the nominal output value given a state and corresponding control input.

Parameters:

Name Type Description Default
x Union[StateInterface, array]

state

 required
u Union[StateInterface, array]

control input

 required

Returns:

Type Description
ndarray

nominal output at (x,u) represented as a numpy array
Source code in commonroad_control/simulation/sensor_models/sensor_model_interface.py 
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
def nominal_output(self, x: Union[StateInterface, np.array], u: Union[StateInterface, np.array]) -> np.ndarray:
    """
    Evaluates the nominal output value given a state and corresponding control input.
    :param x: state
    :param u: control input
    :return: nominal output at (x,u) represented as a numpy array
    """

    # convert state and input to arrays
    if isinstance(x, StateInterface):
        x_np = x.convert_to_array()
    else:
        x_np = x

    if isinstance(u, InputInterface):
        u_np = u.convert_to_array()
    else:
        u_np = u

    # evaluate output function
    x_next = self._output_function(x_np, u_np).full()

    return x_next.squeeze()

commonroad_control.simulation.sensor_models.full_state_feedback.full_state_feedback

FullStateFeedback

Bases: SensorModelInterface

Full state feedback sensor model, i.e., the output function is y = x + n, where n denotes (random) measurement noise.

Source code in commonroad_control/simulation/sensor_models/full_state_feedback/full_state_feedback.py 
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
class FullStateFeedback(SensorModelInterface):
    """
    Full state feedback sensor model, i.e., the output function is y = x + n, where n denotes (random) measurement noise.
    """

    def __init__(
        self,
        noise_model: UncertaintyModelInterface,
        state_output_factory: StateInputDisturbanceTrajectoryFactoryInterface,
        state_dimension: int,
        input_dimension: int,
    ):
        """
        Initialize sensor model.
        :param noise_model: uncertainty model representing sensor noise - UncertaintyModelInterface
        :param state_output_factory: factory for creating States or Outputs as output arguments - StateInputDisturbanceTrajectoryFactoryInterface
        :param state_dimension: state dimension - int
        :param input_dimension: input dimension - int
        """

        # init base class
        super().__init__(
            noise_model=noise_model,
            state_output_factory=state_output_factory,
            dim=state_dimension,
            state_dimension=state_dimension,
            input_dimension=input_dimension,
        )

        # sanity check
        self._sanity_check()

    def _output_function_cas(
        self, x: Union[np.array, cas.SX.sym], u: Union[np.array, cas.SX.sym]
    ) -> Union[cas.SX.sym, np.array]:
        """
        Implements the nominal output function y=x.
        :param x: state
        :param u: control input
        :return: value of the output function evaluated at x, u
        """
        return x

    def _sanity_check(self):
        """
        Check args.
        """

        # dimension of noise model must match dimension of the output
        if self._noise_model.dim != self._dim != self.state_dimension:
            logger.error("Dimensions of noise, output, and state do not match but must be identical.")
            raise ValueError("Dimensions of noise, output, and state do not match but must be identical.")
        # state_output_factory is of correct type
        if not isinstance(self._state_output_factory, StateInputDisturbanceTrajectoryFactoryInterface):
            logger.error(
                f"x must be of type {StateInputDisturbanceTrajectoryFactoryInterface.__name__}, "
                f"not {type(self._state_output_factory).__name__}"
            )
            raise TypeError(
                f"x must be of type {StateInputDisturbanceTrajectoryFactoryInterface.__name__}, "
                f"not {type(self._state_output_factory).__name__}"
            )
        # state dimension of state_output_factory must match state dimension
        if self._dim != self._state_output_factory.state_dimension:
            logger.error("Dimension of output does not match the dimension of the state/output factory.")
            raise ValueError("Dimension of output does not match the dimension of the state/output factory.")

    def measure(self, x: StateInterface, u: InputInterface, rand_noise: bool = True) -> StateInterface:
        """
        Evaluates the output function and applies (random) noise to the output.
        :param x: state
        :param u: input
        :param rand_noise: true, if random noise shall be applied, otherwise the nominal value of the uncertainty model is applied.
        :return: (noisy) measurement
        """

        # check input arguments
        if x.dim != self._state_dimension:
            logger.error(f"Dimension of state {x.dim} does not match.")
            raise ValueError(f"Dimension of state {x.dim} does not match.")
        if u.dim != self._input_dimension:
            logger.error(f"Dimension of input {u.dim} does not match.")
            raise ValueError(f"Dimension of input {u.dim} does not match.")

        # evaluate nominal output
        y_nom_np = self.nominal_output(x, u)

        # sample and apply noise
        if rand_noise:
            noise_np = self._noise_model.sample_uncertainty()
        else:
            noise_np = self._noise_model.nominal_value
        y_np = y_nom_np + noise_np

        # instantiate state interface object as output
        y = self._state_output_factory.state_from_numpy_array(y_np)

        return y
__init__(noise_model, state_output_factory, state_dimension, input_dimension)

Initialize sensor model.

Parameters:

Name Type Description Default
noise_model UncertaintyModelInterface

uncertainty model representing sensor noise - UncertaintyModelInterface

 required
state_output_factory StateInputDisturbanceTrajectoryFactoryInterface

factory for creating States or Outputs as output arguments - StateInputDisturbanceTrajectoryFactoryInterface

 required
state_dimension int

state dimension - int

 required
input_dimension int

input dimension - int

 required
Source code in commonroad_control/simulation/sensor_models/full_state_feedback/full_state_feedback.py 
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
def __init__(
    self,
    noise_model: UncertaintyModelInterface,
    state_output_factory: StateInputDisturbanceTrajectoryFactoryInterface,
    state_dimension: int,
    input_dimension: int,
):
    """
    Initialize sensor model.
    :param noise_model: uncertainty model representing sensor noise - UncertaintyModelInterface
    :param state_output_factory: factory for creating States or Outputs as output arguments - StateInputDisturbanceTrajectoryFactoryInterface
    :param state_dimension: state dimension - int
    :param input_dimension: input dimension - int
    """

    # init base class
    super().__init__(
        noise_model=noise_model,
        state_output_factory=state_output_factory,
        dim=state_dimension,
        state_dimension=state_dimension,
        input_dimension=input_dimension,
    )

    # sanity check
    self._sanity_check()
_output_function_cas(x, u)

Implements the nominal output function y=x.

Parameters:

Name Type Description Default
x Union[array, sym]

state

 required
u Union[array, sym]

control input

 required

Returns:

Type Description
Union[sym, array]

value of the output function evaluated at x, u
Source code in commonroad_control/simulation/sensor_models/full_state_feedback/full_state_feedback.py 
54
55
56
57
58
59
60
61
62
63
def _output_function_cas(
    self, x: Union[np.array, cas.SX.sym], u: Union[np.array, cas.SX.sym]
) -> Union[cas.SX.sym, np.array]:
    """
    Implements the nominal output function y=x.
    :param x: state
    :param u: control input
    :return: value of the output function evaluated at x, u
    """
    return x
_sanity_check()

Check args.

Source code in commonroad_control/simulation/sensor_models/full_state_feedback/full_state_feedback.py 
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
def _sanity_check(self):
    """
    Check args.
    """

    # dimension of noise model must match dimension of the output
    if self._noise_model.dim != self._dim != self.state_dimension:
        logger.error("Dimensions of noise, output, and state do not match but must be identical.")
        raise ValueError("Dimensions of noise, output, and state do not match but must be identical.")
    # state_output_factory is of correct type
    if not isinstance(self._state_output_factory, StateInputDisturbanceTrajectoryFactoryInterface):
        logger.error(
            f"x must be of type {StateInputDisturbanceTrajectoryFactoryInterface.__name__}, "
            f"not {type(self._state_output_factory).__name__}"
        )
        raise TypeError(
            f"x must be of type {StateInputDisturbanceTrajectoryFactoryInterface.__name__}, "
            f"not {type(self._state_output_factory).__name__}"
        )
    # state dimension of state_output_factory must match state dimension
    if self._dim != self._state_output_factory.state_dimension:
        logger.error("Dimension of output does not match the dimension of the state/output factory.")
        raise ValueError("Dimension of output does not match the dimension of the state/output factory.")
measure(x, u, rand_noise=True)

Evaluates the output function and applies (random) noise to the output.

Parameters:

Name Type Description Default
x StateInterface

state

 required
u InputInterface

input

 required
rand_noise bool

true, if random noise shall be applied, otherwise the nominal value of the uncertainty model is applied.

 True

Returns:

Type Description
StateInterface

(noisy) measurement
Source code in commonroad_control/simulation/sensor_models/full_state_feedback/full_state_feedback.py 
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
def measure(self, x: StateInterface, u: InputInterface, rand_noise: bool = True) -> StateInterface:
    """
    Evaluates the output function and applies (random) noise to the output.
    :param x: state
    :param u: input
    :param rand_noise: true, if random noise shall be applied, otherwise the nominal value of the uncertainty model is applied.
    :return: (noisy) measurement
    """

    # check input arguments
    if x.dim != self._state_dimension:
        logger.error(f"Dimension of state {x.dim} does not match.")
        raise ValueError(f"Dimension of state {x.dim} does not match.")
    if u.dim != self._input_dimension:
        logger.error(f"Dimension of input {u.dim} does not match.")
        raise ValueError(f"Dimension of input {u.dim} does not match.")

    # evaluate nominal output
    y_nom_np = self.nominal_output(x, u)

    # sample and apply noise
    if rand_noise:
        noise_np = self._noise_model.sample_uncertainty()
    else:
        noise_np = self._noise_model.nominal_value
    y_np = y_nom_np + noise_np

    # instantiate state interface object as output
    y = self._state_output_factory.state_from_numpy_array(y_np)

    return y

commonroad_control.vehicle_dynamics.full_state_noise_interface

FullStateNoiseInterface dataclass

Bases: UncertaintyInterface

Abstract base class for full state noise - required for the full state feedback sensor model.

Source code in commonroad_control/vehicle_dynamics/full_state_noise_interface.py 
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
@dataclass
class FullStateNoiseInterface(UncertaintyInterface):
    """
    Abstract base class for full state noise - required for the full state feedback sensor model.
    """

    @property
    def dim(self) -> int:
        """
        :return: noise dimension
        """
        return FullStateNoiseInterfaceIndex.dim

    @abstractmethod
    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,)
        """
        pass
dim property

Returns:

Type Description
int

noise dimension
convert_to_array() abstractmethod

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/full_state_noise_interface.py 
34
35
36
37
38
39
40
@abstractmethod
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """
    pass

FullStateNoiseInterfaceIndex dataclass

Bases: UncertaintyInterfaceIndex

Indices of the noise variables.

Source code in commonroad_control/vehicle_dynamics/full_state_noise_interface.py 
12
13
14
15
16
17
18
@dataclass(frozen=True)
class FullStateNoiseInterfaceIndex(UncertaintyInterfaceIndex):
    """
    Indices of the noise variables.
    """

    dim: int

commonroad_control.vehicle_dynamics.double_integrator.di_noise

DINoise dataclass

Bases: FullStateNoiseInterface

Full state noise of the double integrator model - required for the full state feedback sensor model.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_noise.py 
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
@dataclass
class DINoise(FullStateNoiseInterface):
    """
    Full state noise of the double integrator model - required for the full state feedback sensor model.
    """

    position_long: float = 0.0
    position_lat: float = 0.0
    velocity_long: float = 0.0
    velocity_lat: float = 0.0

    @property
    def dim(self) -> int:
        """
        :return: noise dimension
        """
        return DINoiseIndices.dim

    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,)
        """
        y_np = np.zeros((self.dim,))
        y_np[DINoiseIndices.position_long] = self.position_long
        y_np[DINoiseIndices.position_lat] = self.position_lat
        y_np[DINoiseIndices.velocity_long] = self.velocity_long
        y_np[DINoiseIndices.velocity_lat] = self.velocity_lat

        return y_np
dim property

Returns:

Type Description
int

noise dimension
convert_to_array()

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_noise.py 
42
43
44
45
46
47
48
49
50
51
52
53
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """
    y_np = np.zeros((self.dim,))
    y_np[DINoiseIndices.position_long] = self.position_long
    y_np[DINoiseIndices.position_lat] = self.position_lat
    y_np[DINoiseIndices.velocity_long] = self.velocity_long
    y_np[DINoiseIndices.velocity_lat] = self.velocity_lat

    return y_np

DINoiseIndices dataclass

Bases: FullStateNoiseInterfaceIndex

Indices of the noise variables.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_noise.py 
11
12
13
14
15
16
17
18
19
20
21
@dataclass(frozen=True)
class DINoiseIndices(FullStateNoiseInterfaceIndex):
    """
    Indices of the noise variables.
    """

    dim: int = 4
    position_long: int = 0
    position_lat: int = 1
    velocity_long: int = 2
    velocity_lat: int = 3

commonroad_control.vehicle_dynamics.kinematic_bicycle.kb_noise

KBNoise dataclass

Bases: FullStateNoiseInterface

Full state noise of the kinematic bicycle model - required for the full state feedback sensor model.

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_noise.py 
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
@dataclass
class KBNoise(FullStateNoiseInterface):
    """
    Full state noise of the kinematic bicycle model - required for the full state feedback sensor model.
    """

    position_x: float = 0.0
    position_y: float = 0.0
    velocity: float = 0.0
    heading: float = 0.0
    steering_angle: float = 0.0

    @property
    def dim(self) -> int:
        """
        :return: noise dimension
        """
        return KBNoiseIndices.dim

    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,)
        """
        w_np = np.zeros((self.dim,))
        w_np[KBNoiseIndices.position_x] = self.position_x
        w_np[KBNoiseIndices.position_y] = self.position_y
        w_np[KBNoiseIndices.velocity] = self.velocity
        w_np[KBNoiseIndices.heading] = self.heading
        w_np[KBNoiseIndices.steering_angle] = self.steering_angle

        return w_np
dim property

Returns:

Type Description
int

noise dimension
convert_to_array()

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_noise.py 
44
45
46
47
48
49
50
51
52
53
54
55
56
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """
    w_np = np.zeros((self.dim,))
    w_np[KBNoiseIndices.position_x] = self.position_x
    w_np[KBNoiseIndices.position_y] = self.position_y
    w_np[KBNoiseIndices.velocity] = self.velocity
    w_np[KBNoiseIndices.heading] = self.heading
    w_np[KBNoiseIndices.steering_angle] = self.steering_angle

    return w_np

KBNoiseIndices dataclass

Bases: FullStateNoiseInterfaceIndex

Indices of the noise variables.

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_noise.py 
11
12
13
14
15
16
17
18
19
20
21
22
@dataclass(frozen=True)
class KBNoiseIndices(FullStateNoiseInterfaceIndex):
    """
    Indices of the noise variables.
    """

    dim: int = 5
    position_x: int = 0
    position_y: int = 1
    velocity: int = 2
    heading: int = 3
    steering_angle: int = 4

commonroad_control.vehicle_dynamics.dynamic_bicycle.db_noise

DBNoise dataclass

Bases: FullStateNoiseInterface

Full state noise of the dynamic bicycle model - required for the full state feedback sensor model.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_noise.py 
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
@dataclass
class DBNoise(FullStateNoiseInterface):
    """
    Full state noise of the dynamic bicycle model - required for the full state feedback sensor model.
    """

    position_x: float = 0.0
    position_y: float = 0.0
    velocity_long: float = 0.0
    velocity_lat: float = 0.0
    heading: float = 0.0
    yaw_rate: float = 0.0
    steering_angle: float = 0.0

    @property
    def dim(self) -> int:
        """
        :return: noise dimension
        """
        return DBNoiseIndices.dim

    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,)
        """

        y_np = np.zeros((self.dim,))
        y_np[DBNoiseIndices.position_x] = self.position_x
        y_np[DBNoiseIndices.position_y] = self.position_y
        y_np[DBNoiseIndices.velocity_long] = self.velocity_long
        y_np[DBNoiseIndices.velocity_lat] = self.velocity_lat
        y_np[DBNoiseIndices.heading] = self.heading
        y_np[DBNoiseIndices.yaw_rate] = self.yaw_rate
        y_np[DBNoiseIndices.steering_angle] = self.steering_angle

        return y_np
dim property

Returns:

Type Description
int

noise dimension
convert_to_array()

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_noise.py 
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """

    y_np = np.zeros((self.dim,))
    y_np[DBNoiseIndices.position_x] = self.position_x
    y_np[DBNoiseIndices.position_y] = self.position_y
    y_np[DBNoiseIndices.velocity_long] = self.velocity_long
    y_np[DBNoiseIndices.velocity_lat] = self.velocity_lat
    y_np[DBNoiseIndices.heading] = self.heading
    y_np[DBNoiseIndices.yaw_rate] = self.yaw_rate
    y_np[DBNoiseIndices.steering_angle] = self.steering_angle

    return y_np

DBNoiseIndices dataclass

Bases: FullStateNoiseInterfaceIndex

Indices of the noise variables.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_noise.py 
11
12
13
14
15
16
17
18
19
20
21
22
23
24
@dataclass(frozen=True)
class DBNoiseIndices(FullStateNoiseInterfaceIndex):
    """
    Indices of the noise variables.
    """

    dim: int = 7
    position_x: int = 0
    position_y: int = 1
    velocity_long: int = 2
    velocity_lat: int = 3
    heading: int = 4
    yaw_rate: int = 5
    steering_angle: int = 6