Coverage for src/gncpy/dynamics/basic/clohessy_wiltshire_orbit2d.py: 75%

1import numpy as np 

2import gncpy.dynamics._dynamics as cpp_bindings 

3import gncpy.control._control as cpp_control 

4 

5from warnings import warn 

6from .linear_dynamics_base import LinearDynamicsBase 

7 

8 

9class ClohessyWiltshireOrbit2d(LinearDynamicsBase): 

10 """Implements the Clohessy Wiltshire orbit model. 

11 

12 Notes 

13 ----- 

14 This is based on 

15 :cite:`Clohessy1960_TerminalGuidanceSystemforSatelliteRendezvous` 

16 and :cite:`Desai2013_AComparativeStudyofEstimationModelsforSatelliteRelativeMotion` 

17 but only implements the 2D case 

18 

19 Attributes 

20 ---------- 

21 mean_motion :float 

22 mean motion of reference spacecraft 

23 """ 

24 

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

26 

27 def __init__(self, mean_motion=None, **kwargs): 

28 self.mean_motion = mean_motion 

29 

30 super().__init__(**kwargs) 

31 self.__controlParams = cpp_control.ControlParams() 

32 self.__stateTransParams = cpp_bindings.StateTransParams() 

33 

34 if self.mean_motion is not None: 

35 self.__model = cpp_bindings.ClohessyWiltshire2D(0.01, mean_motion) 

36 else: 

37 self.__model = cpp_bindings.ClohessyWiltshire2D(0.01, 0.0) 

38 if "control_model" in kwargs and kwargs["control_model"] is not None: 

39 self.__model.set_control_model(kwargs("control_model")) 

40 

41 @property 

42 def allow_cpp(self): 

43 return True 

44 

45 @property 

46 def control_model(self): 

47 warn("viewing the control model is not supported for this class") 

48 return None 

49 

50 @control_model.setter 

51 def control_model(self, model): 

52 if isinstance(model, cpp_control.ILinearControlModel): 

53 self._control_model = model 

54 self.__model.set_control_model(self._control_model) 

55 else: 

56 raise TypeError("must be ILinearControlModel type") 

57 

58 def get_input_mat(self, timestep, *ctrl_args): 

59 """Calculates the input matrix from the control model. 

60 

61 This calculates the jacobian of the control model. If no control model 

62 is specified than it returns a zero matrix. 

63 

64 Parameters 

65 ---------- 

66 timestep : float 

67 current timestep. 

68 state : N x 1 numpy array 

69 current state. 

70 *ctrl_args : tuple 

71 Additional arguments to pass to the control model. 

72 

73 Returns 

74 ------- 

75 N x Nu numpy array 

76 Control input matrix. 

77 """ 

78 if self._control_model is None: 

79 raise RuntimeWarning("Control model is not set.") 

80 self.args_to_params((0.1,), ctrl_args) 

81 return self._control_model.get_input_mat(timestep, self.__controlParams) 

82 

83 # must be provided if allow_cpp is true 

84 def args_to_params(self, state_args, control_args): 

85 if len(state_args) != 1: 

86 raise RuntimeError( 

87 "state args must be only (dt,) not {}".format(repr(state_args)) 

88 ) 

89 

90 if len(control_args) != 0 and self._control_model is None: 

91 warn("Control agruments supplied but no control model specified") 

92 elif self._control_model is not None: 

93 self.__controlParams = self._control_model.args_to_params( 

94 tuple(control_args) 

95 ) 

96 

97 self.__constraintParams = cpp_bindings.ConstraintParams() 

98 

99 # hack since state params is empty but things are set in the model 

100 self.__model.dt = state_args[0] 

101 return self.__stateTransParams, self.__controlParams, self.__constraintParams 

102 

103 @property 

104 def model(self): 

105 return self.__model 

106 

107 @property 

108 def state_names(self): 

109 return self.__model.state_names() 

110 

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

112 if state_args is None: 

113 raise RuntimeError("state_args must be (dt,) not None") 

114 self.__model.dt = state_args[0] 

115 if self._control_model is None: 

116 next_state = self.__model.propagate_state(timestep, state).reshape((-1, 1)) 

117 else: 

118 next_state = self.__model.propagate_state( 

119 timestep, state, u, *self.args_to_params(state_args, ctrl_args) 

120 ) 

121 if self.state_constraint is not None: 

122 next_state = self.state_constraint(timestep, next_state) 

123 return next_state.reshape((-1, 1)) 

124 

125 def get_dis_process_noise_mat(self, dt, proc_cov): 

126 """Discrete process noise matrix. 

127 

128 Parameters 

129 ---------- 

130 dt : float 

131 time difference, unused. 

132 proc_cov : N x N numpy array 

133 Covariance matrix of the process noise. 

134 

135 Returns 

136 ------- 

137 4 x 4 numpy array 

138 process noise matrix. 

139 

140 """ 

141 gamma = np.array([0, 0, 1, 1]).reshape((4, 1)) 

142 return gamma @ proc_cov @ gamma.T 

143 

144 def get_state_mat(self, timestep, dt): 

145 """Calculates the state transition matrix. 

146 

147 Parameters 

148 ---------- 

149 timestep : float 

150 current timestep. 

151 dt : float 

152 time difference. 

153 

154 Returns 

155 ------- 

156 F : 4 x 4 numpy array 

157 state transition matrix. 

158 

159 """ 

160 self.__model.dt = dt 

161 return self.__model.get_state_mat(timestep, self.__stateTransParams) 

162 # n = self.mean_motion 

163 # c_dtn = np.cos(dt * n) 

164 # s_dtn = np.sin(dt * n) 

165 # F = np.array( 

166 # [ 

167 # [4 - 3 * c_dtn, 0, s_dtn / n, -(2 * c_dtn - 2) / n], 

168 # [ 

169 # 6 * s_dtn - 6 * dt * n, 

170 # 1, 

171 # (2 * c_dtn - 2) / n, 

172 # (4 * s_dtn - 3 * dt * n) / n, 

173 # ], 

174 # [3 * n * s_dtn, 0, c_dtn, 2 * s_dtn], 

175 # [6 * n * (c_dtn - 1), 0, -2 * s_dtn, 4 * c_dtn - 3], 

176 # ] 

177 # ) 

178 # return F