Coverage for src/gncpy/dynamics/basic/coordinated_turn_known.py: 82%

1import numpy as np 

2from .linear_dynamics_base import LinearDynamicsBase 

3 

4 

5class CoordinatedTurnKnown(LinearDynamicsBase): 

6 """Implements the linear coordinated turn with known turn rate model. 

7 

8 This is a slight variation from normal :class:`.LinearDynamicsBase` classes 

9 because it does not allow for control models, instead it has the input 

10 matrix coded directly. It also has the turn angle included in the state 

11 to help with debugging and coding but is not strictly required by the 

12 dynamics. 

13 

14 Notes 

15 ----- 

16 See :cite:`Li2000_SurveyofManeuveringTargetTrackingDynamicModels` and 

17 :cite:`Blackman1999_DesignandAnalysisofModernTrackingSystems` for details. 

18 

19 Attributes 

20 ---------- 

21 turn_rate : float 

22 Turn rate in rad/s 

23 """ 

24 

25 state_names = ("x pos", "y pos", "x vel", "y vel", "turn angle") 

26 

27 def __init__(self, turn_rate=5 * np.pi / 180, **kwargs): 

28 super().__init__(**kwargs) 

29 self.turn_rate = turn_rate 

30 self._control_model = None 

31 

32 @property 

33 def control_model(self): 

34 return self._control_model 

35 

36 @control_model.setter 

37 def control_model(self, model): 

38 self._control_model = model 

39 

40 def get_state_mat(self, timestep, dt): 

41 """Returns the discrete time state matrix. 

42 

43 Parameters 

44 ---------- 

45 timestep : float 

46 timestep. 

47 dt : float 

48 time difference 

49 

50 Returns 

51 ------- 

52 N x N numpy array 

53 state matrix. 

54 """ 

55 # avoid division by 0 

56 if abs(self.turn_rate) < 1e-8: 

57 return np.array( 

58 [ 

59 [1, 0, dt, 0, 0], 

60 [0, 1, 0, dt, 0], 

61 [0, 0, 1, 0, 0], 

62 [0, 0, 0, 1, 0], 

63 [0, 0, 0, 0, 1], 

64 ] 

65 ) 

66 ta = self.turn_rate * dt 

67 s_ta = np.sin(ta) 

68 c_ta = np.cos(ta) 

69 return np.array( 

70 [ 

71 [1, 0, s_ta / self.turn_rate, -(1 - c_ta) / self.turn_rate, 0], 

72 [0, 1, (1 - c_ta) / self.turn_rate, s_ta / self.turn_rate, 0], 

73 [0, 0, c_ta, -s_ta, 0], 

74 [0, 0, s_ta, c_ta, 0], 

75 [0, 0, 0, 0, 1], 

76 ] 

77 ) 

78 

79 def get_input_mat(self, timestep, *args): 

80 """Gets the input matrix. 

81 

82 This enforces the no control model requirement of these dynamics by 

83 forcing the input matrix to be zeros. 

84 

85 Parameters 

86 ---------- 

87 timestep : float 

88 Current timestep. 

89 *args : tuple 

90 additional arguments, not used. 

91 

92 Returns 

93 ------- 

94 N x 1 numpy array 

95 input matrix. 

96 """ 

97 return np.zeros((5, 1)) 

98 

99 def propagate_state(self, timestep, state, u=None, state_args=None, ctrl_args=None): 

100 """Propagates the state forward in time. 

101 

102 Parameters 

103 ---------- 

104 timestep : float 

105 Current timestep. 

106 state : N x 1 numpy array 

107 current state. 

108 u : 1 x 1 numpy array, optional 

109 Control input, should not be needed with this model. The default is 

110 None. 

111 state_args : tuple, optional 

112 Additional arguments needed to get the state matrix. The default is 

113 None. 

114 ctrl_args : tuple, optional 

115 Additional arguments needed to get the input matrix, not needed. 

116 The default is None. 

117 

118 Raises 

119 ------ 

120 RuntimeError 

121 If state_args is None. 

122 

123 Returns 

124 ------- 

125 N x 1 numpy array 

126 Next state. 

127 """ 

128 if state_args is None: 

129 raise RuntimeError("state_args must be (dt, )") 

130 if ctrl_args is None: 

131 ctrl_args = () 

132 F = self.get_state_mat( 

133 timestep, 

134 *state_args, 

135 ) 

136 if u is None: 

137 return F @ state + np.array( 

138 [0, 0, 0, 0, state_args[0] * self.turn_rate] 

139 ).reshape((5, 1)) 

140 G = self.get_input_mat(timestep, *ctrl_args) 

141 return ( 

142 F @ state 

143 + G @ u 

144 + np.array([0, 0, 0, 0, state_args[0] * self.turn_rate]).reshape((5, 1)) 

145 )