Hide keyboard shortcuts

Hot-keys on this page

r m x p   toggle line displays

j k   next/prev highlighted chunk

0   (zero) top of page

1   (one) first highlighted chunk

1"""Blocks for linear, time-invariant systems""" 

2import numpy as np 

3from functools import partial 

4from modypy.model import Block, Port, Signal, SignalState, State 

5 

6 

7class InvalidLTIException(RuntimeError): 

8 """An exception which is raised when the specification of an LTI is invalid. 

9 """ 

10 

11 

12class LTISystem(Block): 

13 """Implementation of a linear, time-invariant block of the following format: 

14 

15 dx/dt = system_matrix * x + input_matrix * u 

16 y = output_matrix * x + feed_through_matrix * u 

17 

18 The matrices ``system_matrix``, ``input_matrix``, ``output_matrix`` and 

19 ``feed_through_matrix`` define the state and output behaviour of the block. 

20 """ 

21 

22 def __init__(self, 

23 parent, 

24 system_matrix, 

25 input_matrix, 

26 output_matrix, 

27 feed_through_matrix, 

28 initial_condition=None): 

29 Block.__init__(self, parent) 

30 

31 # Determine the number of states and the shape of the state 

32 if np.isscalar(system_matrix): 32 ↛ 33line 32 didn't jump to line 33, because the condition on line 32 was never true

33 self.state_shape = () 

34 num_states = 1 

35 else: 

36 system_matrix = np.asarray(system_matrix) 

37 if (system_matrix.ndim == 2 and 37 ↛ 40line 37 didn't jump to line 40, because the condition on line 37 was never true

38 system_matrix.shape[0] > 0 and 

39 system_matrix.shape[0] == system_matrix.shape[1]): 

40 self.state_shape = system_matrix.shape[0] 

41 num_states = self.state_shape 

42 else: 

43 raise InvalidLTIException('The system matrix must be a scalar ' 

44 'or a non-empty square matrix') 

45 

46 # Determine the number of inputs and the shape of the input signal 

47 if np.isscalar(input_matrix): 

48 if num_states > 1: 

49 raise InvalidLTIException('There is more than one state, but ' 

50 'the input matrix is neither empty, ' 

51 'nor a vector or a matrix') 

52 num_inputs = 1 

53 self.input_shape = () 

54 else: 

55 input_matrix = np.asarray(input_matrix) 

56 if input_matrix.ndim == 0: 

57 # There are no inputs 

58 num_inputs = 0 

59 elif input_matrix.ndim == 1: 

60 # The input matrix is a column vector 

61 if num_states > 1: 

62 raise InvalidLTIException('There is more than one state, ' 

63 'but the input matrix is neither ' 

64 'empty nor a vector or a matrix') 

65 num_inputs = input_matrix.shape[0] 

66 elif input_matrix.ndim == 2: 

67 # The input matrix is a matrix 

68 if num_states != input_matrix.shape[0]: 

69 raise InvalidLTIException('The height of the input matrix ' 

70 'does not match the number of ' 

71 'states') 

72 num_inputs = input_matrix.shape[1] 

73 else: 

74 raise InvalidLTIException('The input matrix must be empty,' 

75 'a scalar, a vector or a matrix') 

76 self.input_shape = num_inputs 

77 

78 # Determine the number of outputs and the shape of the output array 

79 if np.isscalar(output_matrix): 

80 if num_states > 1: 

81 raise InvalidLTIException('There is more than one state, but ' 

82 'the output matrix is neither an ' 

83 'empty, a vector nor a matrix') 

84 num_outputs = 1 

85 self.output_shape = () 

86 else: 

87 output_matrix = np.asarray(output_matrix) 

88 if output_matrix.ndim == 0: 

89 # There are no outputs 

90 num_outputs = 0 

91 elif output_matrix.ndim == 1: 

92 # The output matrix is a row vector 

93 if num_states != output_matrix.shape[0]: 

94 raise InvalidLTIException('The width of the output matrix ' 

95 'does not match the number of ' 

96 'states') 

97 num_outputs = output_matrix.shape[0] 

98 elif output_matrix.ndim == 2: 

99 # The output matrix is a matrix 

100 if num_states != output_matrix.shape[1]: 

101 raise InvalidLTIException('The width of the output matrix ' 

102 'does not match the number of ' 

103 'states') 

104 num_outputs = output_matrix.shape[0] 

105 else: 

106 raise InvalidLTIException('The output matrix must be empty, a' 

107 'scalar, a vector or a matrix') 

108 self.output_shape = num_outputs 

109 

110 if np.isscalar(feed_through_matrix): 

111 if not (num_inputs == 1 and num_outputs == 1): 

112 raise InvalidLTIException('A scalar feed-through matrix is ' 

113 'only allowed for systems with ' 

114 'exactly one input and one output') 

115 else: 

116 feed_through_matrix = np.asarray(feed_through_matrix) 

117 if feed_through_matrix.ndim == 0: 

118 if not (num_inputs == 0 or num_outputs == 0): 

119 raise InvalidLTIException('The feed-through matrix for a ' 

120 'system with both inputs and ' 

121 'outputs must be a scalar, a ' 

122 'vector or a matrix') 

123 elif feed_through_matrix.ndim == 1: 

124 # A vector feed_through_matrix is interpreted as row vector, 

125 # so there must be exactly one output. 

126 if num_outputs == 0: 

127 raise InvalidLTIException('The feed-through matrix for a ' 

128 'system without outputs must be' 

129 'empty') 

130 elif num_outputs > 1: 

131 raise InvalidLTIException('The feed-through matrix for a ' 

132 'system with more than one ' 

133 'output must be a matrix') 

134 if feed_through_matrix.shape[0] != num_inputs: 

135 raise InvalidLTIException('The width of the feed-through ' 

136 'matrix must match the number of ' 

137 'inputs') 

138 elif feed_through_matrix.ndim == 2: 

139 if feed_through_matrix.shape[0] != num_outputs: 

140 raise InvalidLTIException('The height of the feed-through ' 

141 'matrix must match the number of ' 

142 'outputs') 

143 if feed_through_matrix.shape[1] != num_inputs: 

144 raise InvalidLTIException('The width of the feed-through ' 

145 'matrix must match the number of ' 

146 'inputs') 

147 

148 self.system_matrix = system_matrix 

149 self.input_matrix = input_matrix 

150 self.output_matrix = output_matrix 

151 self.feed_through_matrix = feed_through_matrix 

152 

153 self.input = Port(shape=self.input_shape) 

154 self.state = State(self, 

155 shape=self.state_shape, 

156 derivative_function=self.state_derivative, 

157 initial_condition=initial_condition) 

158 self.output = Signal(shape=self.output_shape, 

159 value=self.output_function) 

160 

161 def state_derivative(self, data): 

162 """Calculates the state derivative for the system""" 

163 if self.state.shape == (): 

164 derivative = self.system_matrix * self.state(data) 

165 else: 

166 derivative = np.matmul(self.system_matrix, self.state(data)) 

167 if self.input.shape == (): 

168 derivative += self.input_matrix * self.input(data) 

169 elif self.input.size > 0: 

170 derivative += np.matmul(self.input_matrix, self.input(data)) 

171 return derivative 

172 

173 def output_function(self, data): 

174 """Calculates the output for the system""" 

175 if self.state.shape == (): 

176 output = self.output_matrix * self.state(data) 

177 else: 

178 output = np.matmul(self.output_matrix, self.state(data)) 

179 if self.input.shape == (): 

180 output += self.feed_through_matrix * self.input(data) 

181 elif self.input.size > 0: 

182 output += np.matmul(self.feed_through_matrix, self.input(data)) 

183 return output 

184 

185 

186class Gain(Block): 

187 """A simple linear gain block. 

188 

189 Provides the input scaled by the constant gain as output. 

190 

191 This class is deprecated. Use ``gain`` instead. 

192 """ 

193 

194 def __init__(self, parent, k): 

195 Block.__init__(self, parent) 

196 self.k = np.atleast_2d(k) 

197 

198 self.input = Port(shape=self.k.shape[0]) 

199 self.output = Signal(shape=self.k.shape[1], 

200 value=self.output_function) 

201 

202 def output_function(self, data): 

203 """Calculates the output for the system 

204 

205 Args: 

206 data: The current time, states and signals for the system. 

207 

208 Returns: The input multiplied by the gain 

209 """ 

210 return self.k @ self.input(data) 

211 

212 

213def _gain_function(gain_matrix, input_signal, data): 

214 """ 

215 Calculate the product of the given gain matrix and the value of the signal. 

216 

217 Args: 

218 gain_matrix: The gain (matrix) to apply 

219 input_signal: The input signal 

220 data: The data provider 

221 

222 Returns: 

223 The product of the gain matrix and the value of the signal 

224 """ 

225 

226 return np.matmul(gain_matrix, input_signal(data)) 

227 

228 

229def gain(gain_matrix, input_signal): 

230 """ 

231 Create a signal that represents the product of the given gain matrix and the 

232 value of the given input signal. 

233 

234 Args: 

235 gain_matrix: The gain matrix 

236 input_signal: The input signal to consider 

237 

238 Returns: 

239 A signal that represents the product of the gain matrix and the value of 

240 the input signal. 

241 """ 

242 

243 # Determine the shape of the output signal 

244 output_shape = (gain_matrix @ np.zeros(input_signal.shape)).shape 

245 return Signal(shape=output_shape, 

246 value=partial(_gain_function, 

247 gain_matrix, 

248 input_signal)) 

249 

250 

251class Sum(Block): 

252 """A linear weighted sum block. 

253 

254 This block may have a number of inputs which are interpreted as vectors of 

255 common dimension. The output of the block is calculated as the weighted 

256 sum of the inputs. 

257 

258 The ``channel_weights`` give the factors by which the individual channels 

259 are weighted in the sum. 

260 

261 This class is deprecated. Use ``sum_signal`` instead. 

262 """ 

263 

264 def __init__(self, 

265 parent, 

266 channel_weights, 

267 output_size=1): 

268 Block.__init__(self, parent) 

269 

270 self.channel_weights = np.asarray(channel_weights) 

271 self.output_size = output_size 

272 

273 self.inputs = [Port(shape=self.output_size) 

274 for _ in range(self.channel_weights.shape[0])] 

275 self.output = Signal(shape=self.output_size, 

276 value=self.output_function) 

277 

278 def output_function(self, data): 

279 """Calculates the output for the system 

280 

281 Args: 

282 data: The time, states and signals of the system 

283 

284 Returns: 

285 The sum of the input signals 

286 """ 

287 inputs = np.empty((len(self.inputs), self.output_size)) 

288 for port_idx in range(len(self.inputs)): 

289 inputs[port_idx] = self.inputs[port_idx](data) 

290 return self.channel_weights @ inputs 

291 

292 

293def _sum_function(signals, gains, data): 

294 """ 

295 Calculate the sum of the values of the given signals multiplied by the 

296 given gains. 

297 

298 Args: 

299 signals: A tuple of signals 

300 gains: A tuple of gains 

301 data: The data provider 

302 

303 Returns: 

304 The sum of the values of the given signals multiplied by the given 

305 gains 

306 """ 

307 

308 signal_sum = 0 

309 for signal, gain_value in zip(signals, gains): 

310 signal_sum = signal_sum + np.dot(gain_value, signal(data)) 

311 return signal_sum 

312 

313 

314def sum_signal(input_signals, gains=None): 

315 """ 

316 Create a signal that represents the sum of the input signals multiplied by 

317 the given gains. 

318 

319 The signals must have the same shape and there must be exactly as many 

320 entries in the ``gains`` tuple as there are input signals. 

321 

322 Args: 

323 input_signals: A tuple of input signals 

324 gains: A tuple of gains for the input signals. Optional: Default value 

325 is all ones. 

326 

327 Returns: 

328 A signal that represents the sum of the input signals 

329 """ 

330 

331 if gains is None: 

332 gains = np.ones(len(input_signals)) 

333 

334 shape = input_signals[0].shape 

335 if any(signal.shape != shape for signal in input_signals): 

336 raise ValueError('The shapes of the input signals do not match') 

337 if len(input_signals) != len(gains): 

338 raise ValueError('There must be as many gains as there are ' 

339 'input signals') 

340 

341 return Signal(shape=shape, 

342 value=partial(_sum_function, 

343 input_signals, 

344 gains)) 

345 

346 

347def integrator(owner, input_signal, initial_condition=None): 

348 """ 

349 Create a state-signal that provides the integrated value of the input 

350 callable. 

351 

352 The resulting signal will have the same shape as the input callable. 

353 

354 Args: 

355 owner: The owner of the integrator 

356 input_signal: A callable accepting an object implementing the system 

357 object access protocol and providing the value of the derivative 

358 initial_condition: The initial condition of the integrator 

359 (default: ``None``) 

360 

361 Returns: 

362 A state-signal that provides the integrated value of the input signal 

363 """ 

364 

365 return SignalState(owner, 

366 shape=input_signal.shape, 

367 derivative_function=input_signal, 

368 initial_condition=initial_condition)