Coverage for src/gncpy/dynamics/basic/coordinated_turn_unknown.py: 79%
57 statements
« prev ^ index » next coverage.py v7.6.1, created at 2024-09-13 06:15 +0000
1import numpy as np
2from warnings import warn
3from .nonlinear_dynamics_base import NonlinearDynamicsBase
6class CoordinatedTurnUnknown(NonlinearDynamicsBase):
7 r"""Implements the non-linear coordinated turn with unknown turn rate model.
9 Notes
10 -----
11 This can use either a Wiener process (:math:`\alpha=1`) or a first order
12 Markov process model (:math:`\alpha \neq 1`) for the turn rate. This is
13 controlled by setting :attr:`.turn_rate_cor_time`. See
14 :cite:`Li2000_SurveyofManeuveringTargetTrackingDynamicModels` and
15 :cite:`Blackman1999_DesignandAnalysisofModernTrackingSystems` for details.
17 .. math::
18 \dot{\omega} = -\alpha w + w_{\omega}
20 Attributes
21 ----------
22 turn_rate_cor_time : float
23 Correlation time for the turn rate. If None then a Wiener process is used.
24 """
26 state_names = ("x pos", "y pos", "x vel", "y vel", "turn rate")
28 def __init__(self, turn_rate_cor_time=None, **kwargs):
29 """Initialize an object.
31 Parameters
32 ----------
33 turn_rate_cor_time : float, optional
34 Correlation time of the turn rate. The default is None.
35 **kwargs : dict
36 Additional arguments for the parent.
37 """
38 super().__init__(**kwargs)
39 self.turn_rate_cor_time = turn_rate_cor_time
41 self._control_model = [None] * len(self.state_names)
43 @property
44 def alpha(self):
45 """Read only inverse of the turn rate correlation time."""
46 if self.turn_rate_cor_time is None:
47 return 0
48 else:
49 return 1 / self.turn_rate_cor_time
51 @property
52 def beta(self):
53 """Read only value for correlation time in state matrix."""
54 if self.turn_rate_cor_time is None:
55 return 1
56 else:
57 return np.exp(-self.dt / self.turn_rate_cor_time)
59 @property
60 def cont_fnc_lst(self):
61 """Continuous time ODEs, not used."""
62 warn("Not used by this class")
63 return []
65 @property
66 def control_model(self):
67 return self._control_model
69 @control_model.setter
70 def control_model(self, model):
71 self._control_model = model
73 def get_state_mat(
74 self, timestep, state, *args, u=None, ctrl_args=None, use_continuous=False
75 ):
76 """Returns the linearized state matrix that has been hardcoded.
78 Parameters
79 ----------
80 timestep : float
81 Current timestep.
82 state : N x 1 numpy array
83 current state.
84 *args : tuple
85 Additional arguments placeholde, not used.
86 u : Nu x 1 numpy array, optional
87 Control input. The default is None.
88 ctrl_args : tuple, optional
89 Additional agruments needed to get the input matrix. The default is
90 None.
91 use_continuous : bool, optional
92 Flag indicating if the continuous time A matrix should be returned.
93 The default is False.
95 Returns
96 -------
97 N x N numpy array
98 state transition matrix.
99 """
100 x = state.ravel()
101 w = x[4]
103 if use_continuous:
104 return np.array(
105 [
106 [0, 0, 1, 0, 0],
107 [0, 0, 0, 1, 0],
108 [0, 0, 0, -w, 0],
109 [0, 0, w, 0, 0],
110 [0, 0, 0, 0, -self.alpha],
111 ]
112 )
113 # avoid division by 0
114 if abs(w) < 1e-8:
115 return np.array(
116 [
117 [1, 0, self.dt, 0, 0],
118 [0, 1, 0, self.dt, 0],
119 [0, 0, 1, 0, 0],
120 [0, 0, 0, 1, 0],
121 [0, 0, 0, 0, self.beta],
122 ]
123 )
124 ta = w * self.dt
125 s_ta = np.sin(ta)
126 c_ta = np.cos(ta)
128 w2 = w**2
129 F04 = (w * self.dt * c_ta - s_ta) * x[2] / w2 - (
130 w * self.dt * s_ta - 1 + c_ta
131 ) * x[3] / w2
132 F14 = (w * self.dt * s_ta - 1 + c_ta) * x[2] / w2 + (
133 w * self.dt * c_ta - s_ta
134 ) * x[3] / w2
135 F24 = -self.dt * s_ta * x[2] - self.dt * c_ta * x[3]
136 F34 = self.dt * c_ta * x[2] - self.dt * s_ta * x[3]
137 return np.array(
138 [
139 [1, 0, s_ta / w, -(1 - c_ta) / w, F04],
140 [0, 1, (1 - c_ta) / w, s_ta / w, F14],
141 [0, 0, c_ta, -s_ta, F24],
142 [0, 0, s_ta, c_ta, F34],
143 [0, 0, 0, 0, self.beta],
144 ]
145 )
147 def get_input_mat(self, timestep, state, u, state_args=None, ctrl_args=None):
148 """Returns the linearized input matrix.
150 This assumes the control input is an AWGN signal.
152 Parameters
153 ----------
154 timestep : float
155 Current timestep.
156 state : N x 1 numpy array
157 Current state.
158 u : 3 x 1 numpy array
159 Current control input.
160 state_args : tuple, optional
161 Additional arguements needed to get the state matrix. The default
162 is None.
163 ctrl_args : tuple, optional
164 Additional arguments needed to get the input matrix. The default is
165 None.
167 Returns
168 -------
169 N x 3 numpy array
170 Input matrix.
171 """
172 return np.array(
173 [
174 [0.5 * self.dt**2, 0, 0],
175 [0, 0.5 * self.dt**2, 0],
176 [self.dt, 0, 0],
177 [0, self.dt, 0],
178 [0, 0, 1],
179 ]
180 )
182 def propagate_state(self, timestep, state, u=None, state_args=None, ctrl_args=None):
183 """Propagates the state forward one timestep.
185 This uses the hardcoded form for the linearized state and input matrices
186 instead of numerical integration. It assumes the control input is an
187 AWGN signal.
189 Parameters
190 ----------
191 timestep : float
192 Current timestep.
193 state : N x 1 numpy array
194 current state.
195 u : 3 x 1 numpy array, optional
196 Control input. The default is None.
197 state_args : tuple, optional
198 Additional arguements needed to get the state matrix. The default
199 is None. These are not needed.
200 ctrl_args : tuple, optional
201 Additional arguments needed to get the input matrix. The default is
202 None. These are not needed.
204 Returns
205 -------
206 N x 1 numpy array
207 Next state.
208 """
209 if state_args is None:
210 state_args = ()
211 if ctrl_args is None:
212 ctrl_args = ()
213 F = self.get_state_mat(
214 timestep, state, *state_args, u=u, ctrl_args=ctrl_args, use_continuous=False
215 )
216 if u is None:
217 return F @ state
218 G = self.get_input_mat(
219 timestep, state, u, state_args=state_args, ctrl_args=ctrl_args
220 )
221 return F @ state + G @ u