Coverage for src/gncpy/dynamics/basic/curvilinear_motion.py: 73%
49 statements
« prev ^ index » next coverage.py v7.2.7, created at 2023-07-19 05:48 +0000
1import numpy as np
3from warnings import warn
4from .nonlinear_dynamics_base import NonlinearDynamicsBase
8class CurvilinearMotion(NonlinearDynamicsBase):
9 r"""Implements general curvilinear motion model in 2d.
11 This is a slight variation from normal :class:`.NonlinearDynamicsBase` classes
12 because it does not use a list of continuous functions but instead has
13 the state and input matrices coded directly. As a result, it also does not
14 use the control model attribute because it is hardcoded in the
15 :meth:`.get_input_mat` function. Also, the angle state should be kept between 0-360
16 degrees.
18 Notes
19 -----
20 This implements the following system of ODEs.
22 .. math::
24 \begin{align}
25 \dot{x} &= v cos(\psi) \\
26 \dot{y} &= v sin(\psi) \\
27 \dot{v} &= u_0 \\
28 \dot{\psi} &= u_1
29 \end{align}
31 See :cite:`Li2000_SurveyofManeuveringTargetTrackingDynamicModels` for
32 details.
33 """
35 state_names = (
36 "x pos",
37 "y pos",
38 "speed",
39 "turn angle",
40 )
42 def __init__(self, **kwargs):
43 """Initialize an object.
45 Parameters
46 ----------
47 **kwargs : dict
48 Additional key word arguments for the parent.
49 """
50 super().__init__(**kwargs)
51 self._control_model = [None] * len(self.state_names)
52 self.control_constraint = None
54 @property
55 def cont_fnc_lst(self):
56 """Continuous time ODEs, not used."""
57 warn("Not used by this class")
58 return []
60 @property
61 def control_model(self):
62 return self._control_model
64 @control_model.setter
65 def control_model(self, model):
66 self._control_model = model
68 def get_state_mat(
69 self, timestep, state, *args, u=None, ctrl_args=None, use_continuous=False
70 ):
71 """Returns the linearized state matrix that has been hardcoded.
73 Parameters
74 ----------
75 timestep : float
76 Current timestep.
77 state : N x 1 numpy array
78 current state.
79 *args : tuple
80 Additional arguments placeholde, not used.
81 u : Nu x 1 numpy array, optional
82 Control input. The default is None.
83 ctrl_args : tuple, optional
84 Additional agruments needed to get the input matrix. The default is
85 None.
86 use_continuous : bool, optional
87 Flag indicating if the continuous time A matrix should be returned.
88 The default is False.
90 Returns
91 -------
92 N x N numpy array
93 state transition matrix.
94 """
95 x = state.ravel()
97 A = np.array(
98 [
99 [0, 0, np.cos(x[3]), -x[2] * np.sin(x[3]) * self.dt],
100 [0, 0, np.sin(x[3]), x[2] * np.cos(x[3]) * self.dt],
101 [0, 0, 0, 0],
102 [0, 0, 0, 0],
103 ]
104 )
105 if use_continuous:
106 return A
107 return np.eye(A.shape[0]) + self.dt * A
109 def get_input_mat(self, timestep, state, u, state_args=None, ctrl_args=None):
110 """Returns the linearized input matrix.
112 Parameters
113 ----------
114 timestep : float
115 Current timestep.
116 state : N x 1 numpy array
117 Current state.
118 u : 2 x 1 numpy array
119 Current control input.
120 state_args : tuple, optional
121 Additional arguements needed to get the state matrix. The default
122 is None.
123 ctrl_args : tuple, optional
124 Additional arguments needed to get the input matrix. The default is
125 None.
127 Returns
128 -------
129 N x 2 numpy array
130 Input matrix.
131 """
132 return np.array([[0, 0], [0, 0], [self.dt, 0], [0, self.dt]])
134 def propagate_state(self, timestep, state, u=None, state_args=None, ctrl_args=None):
135 """Propagates the state forward one timestep.
137 This uses the hardcoded form for the linearized state and input matrices
138 instead of numerical integration.
140 Parameters
141 ----------
142 timestep : float
143 Current timestep.
144 state : N x 1 numpy array
145 current state.
146 u : 2 x 1 numpy array, optional
147 Control input. The default is None.
148 state_args : tuple, optional
149 Additional arguements needed to get the state matrix. The default
150 is None. These are not needed.
151 ctrl_args : tuple, optional
152 Additional arguments needed to get the input matrix. The default is
153 None. These are not needed.
155 Returns
156 -------
157 N x 1 numpy array
158 Next state.
159 """
160 if state_args is None:
161 state_args = ()
162 if ctrl_args is None:
163 ctrl_args = ()
164 F = self.get_state_mat(
165 timestep, state, *state_args, u=u, ctrl_args=ctrl_args, use_continuous=False
166 )
167 _state = state.copy()
168 _state[3] = np.mod(_state[3], 2 * np.pi)
169 next_state = F @ _state
170 if u is None:
171 if self.state_constraint is not None:
172 next_state = self.state_constraint(timestep, next_state)
173 next_state[3] = np.mod(next_state[3], 2 * np.pi)
174 return next_state
175 G = self.get_input_mat(
176 timestep, state, u, state_args=state_args, ctrl_args=ctrl_args
177 )
178 if self.control_constraint is not None:
179 next_state += G @ self.control_constraint(timestep, u.copy())
180 else:
181 next_state += G @ u
182 if self.state_constraint is not None:
183 next_state = self.state_constraint(timestep, next_state)
184 next_state[3] = np.mod(next_state[3], 2 * np.pi)
185 return next_state