Coverage for src/gncpy/dynamics/basic/linear_dynamics_base.py: 32%

1import numpy as np 

2 

3from warnings import warn 

4from .dynamics_base import DynamicsBase 

5 

6class LinearDynamicsBase(DynamicsBase): 

7 """Base class for all linear dynamics models. 

8 

9 Child classes should define their own get_state_mat function and set the 

10 state names class variable. The remainder of the functions autogenerate 

11 based on these values. 

12 

13 """ 

14 

15 def __init__(self, **kwargs): 

16 super().__init__(**kwargs) 

17 

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

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

20 

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

22 is specified than it returns a zero matrix. 

23 

24 Parameters 

25 ---------- 

26 timestep : float 

27 current timestep. 

28 state : N x 1 numpy array 

29 current state. 

30 *ctrl_args : tuple 

31 Additional arguments to pass to the control model. 

32 

33 Returns 

34 ------- 

35 N x Nu numpy array 

36 Control input matrix. 

37 """ 

38 if self._control_model is None: 

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

40 return self._control_model(timestep, *ctrl_args) 

41 

42 def get_dis_process_noise_mat(self, dt, *f_args): 

43 """Class method for getting the process noise. 

44 

45 Should be overridden in child classes. Should maintain the same 

46 signature to allow for standardized wrappers. 

47 

48 Parameters 

49 ---------- 

50 dt : float 

51 delta time. 

52 *kwargs : tuple, optional 

53 any additional arguments needed. 

54 

55 Returns 

56 ------- 

57 N x N numpy array 

58 discrete time process noise matrix. 

59 

60 """ 

61 msg = "get_dis_process_noise_mat function is undefined" 

62 warn(msg, RuntimeWarning) 

63 return np.array([[]]) 

64 

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

66 r"""Propagates the state to the next time step. 

67 

68 Notes 

69 ----- 

70 This implements the equation 

71 

72 .. math:: 

73 x_{k+1} = F_k x_k + G_k u_k 

74 

75 Parameters 

76 ---------- 

77 timestep : float 

78 timestep. 

79 state : N x 1 numpy array 

80 state vector. 

81 u : Nu x 1 numpy array, optional 

82 Control effort vector. The default is None. 

83 state_args : tuple, optional 

84 Additional arguments needed by the `get_state_mat` function. The 

85 default is (). 

86 ctrl_args : tuple, optional 

87 Additional arguments needed by the get input mat function. The 

88 default is (). Only used if a control effort is supplied. 

89 

90 Returns 

91 ------- 

92 next_state : N x 1 numpy array 

93 The propagated state. 

94 

95 """ 

96 if state_args is None: 

97 state_args = () 

98 if ctrl_args is None: 

99 ctrl_args = () 

100 state_trans_mat = self.get_state_mat(timestep, *state_args) 

101 next_state = state_trans_mat @ state 

102 

103 if self.control_model is not None: 

104 

105 input_mat = self.control_model(timestep, state, *ctrl_args) 

106 ctrl = input_mat @ u 

107 next_state += ctrl 

108 if self.state_constraint is not None: 

109 next_state = self.state_constraint(timestep, next_state) 

110 return next_state