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): 

33 self.state_shape = () 

34 num_states = 1 

35 else: 

36 system_matrix = np.asarray(system_matrix) 

37 if (system_matrix.ndim == 2 and 

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 == 1: 

57 # The input matrix is a column vector => one input 

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

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

60 'must match the number of states') 

61 num_inputs = 1 

62 elif input_matrix.ndim == 2: 

63 # The input matrix is a matrix 

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

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

66 'does not match the number of ' 

67 'states') 

68 num_inputs = input_matrix.shape[1] 

69 else: 

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

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

72 self.input_shape = num_inputs 

73 

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

75 if np.isscalar(output_matrix): 

76 if num_states > 1: 

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

78 'the output matrix is neither an ' 

79 'empty, a vector nor a matrix') 

80 num_outputs = 1 

81 self.output_shape = () 

82 else: 

83 output_matrix = np.asarray(output_matrix) 

84 if output_matrix.ndim == 1: 

85 # The output matrix is a row vector => one output 

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

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

88 'does not match the number of ' 

89 'states') 

90 num_outputs = 1 

91 elif output_matrix.ndim == 2: 

92 # The output matrix is a matrix 

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

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 else: 

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

100 'scalar, a vector or a matrix') 

101 self.output_shape = num_outputs 

102 

103 if np.isscalar(feed_through_matrix): 

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

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

106 'only allowed for systems with ' 

107 'exactly one input and one output') 

108 else: 

109 feed_through_matrix = np.asarray(feed_through_matrix) 

110 if feed_through_matrix.ndim == 1: 

111 # A vector feed_through_matrix is interpreted as row vector, 

112 # so there must be exactly one output. 

113 if num_outputs == 0: 

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

115 'system without outputs must be' 

116 'empty') 

117 elif num_outputs > 1: 

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

119 'system with more than one ' 

120 'output must be a matrix') 

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

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

123 'matrix must match the number of ' 

124 'inputs') 

125 elif feed_through_matrix.ndim == 2: 

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

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

128 'matrix must match the number of ' 

129 'outputs') 

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

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

132 'matrix must match the number of ' 

133 'inputs') 

134 else: 

135 raise InvalidLTIException('The feed-through matrix must be ' 

136 'empty, a scalar, a vector or a ' 

137 'matrix') 

138 

139 self.system_matrix = system_matrix 

140 self.input_matrix = input_matrix 

141 self.output_matrix = output_matrix 

142 self.feed_through_matrix = feed_through_matrix 

143 

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

145 self.state = State(self, 

146 shape=self.state_shape, 

147 derivative_function=self.state_derivative, 

148 initial_condition=initial_condition) 

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

150 value=self.output_function) 

151 

152 def state_derivative(self, data): 

153 """Calculates the state derivative for the system""" 

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

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

156 else: 

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

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

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

160 elif self.input.size > 0: 

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

162 return derivative 

163 

164 def output_function(self, data): 

165 """Calculates the output for the system""" 

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

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

168 else: 

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

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

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

172 elif self.input.size > 0: 

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

174 return output 

175 

176 

177class Gain(Block): 

178 """A simple linear gain block. 

179 

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

181 

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

183 """ 

184 

185 def __init__(self, parent, k): 

186 Block.__init__(self, parent) 

187 self.k = np.atleast_2d(k) 

188 

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

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

191 value=self.output_function) 

192 

193 def output_function(self, data): 

194 """Calculates the output for the system 

195 

196 Args: 

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

198 

199 Returns: The input multiplied by the gain 

200 """ 

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

202 

203 

204def _gain_function(gain_matrix, input_signal, data): 

205 """ 

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

207 

208 Args: 

209 gain_matrix: The gain (matrix) to apply 

210 input_signal: The input signal 

211 data: The data provider 

212 

213 Returns: 

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

215 """ 

216 

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

218 

219 

220def gain(gain_matrix, input_signal): 

221 """ 

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

223 value of the given input signal. 

224 

225 Args: 

226 gain_matrix: The gain matrix 

227 input_signal: The input signal to consider 

228 

229 Returns: 

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

231 the input signal. 

232 """ 

233 

234 # Determine the shape of the output signal 

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

236 return Signal(shape=output_shape, 

237 value=partial(_gain_function, 

238 gain_matrix, 

239 input_signal)) 

240 

241 

242class Sum(Block): 

243 """A linear weighted sum block. 

244 

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

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

247 sum of the inputs. 

248 

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

250 are weighted in the sum. 

251 

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

253 """ 

254 

255 def __init__(self, 

256 parent, 

257 channel_weights, 

258 output_size=1): 

259 Block.__init__(self, parent) 

260 

261 self.channel_weights = np.asarray(channel_weights) 

262 self.output_size = output_size 

263 

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

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

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

267 value=self.output_function) 

268 

269 def output_function(self, data): 

270 """Calculates the output for the system 

271 

272 Args: 

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

274 

275 Returns: 

276 The sum of the input signals 

277 """ 

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

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

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

281 return self.channel_weights @ inputs 

282 

283 

284def _sum_function(signals, gains, data): 

285 """ 

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

287 given gains. 

288 

289 Args: 

290 signals: A tuple of signals 

291 gains: A tuple of gains 

292 data: The data provider 

293 

294 Returns: 

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

296 gains 

297 """ 

298 

299 signal_sum = 0 

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

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

302 return signal_sum 

303 

304 

305def sum_signal(input_signals, gains=None): 

306 """ 

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

308 the given gains. 

309 

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

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

312 

313 Args: 

314 input_signals: A tuple of input signals 

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

316 is all ones. 

317 

318 Returns: 

319 A signal that represents the sum of the input signals 

320 """ 

321 

322 if gains is None: 

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

324 

325 shape = input_signals[0].shape 

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

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

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

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

330 'input signals') 

331 

332 return Signal(shape=shape, 

333 value=partial(_sum_function, 

334 input_signals, 

335 gains)) 

336 

337 

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

339 """ 

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

341 callable. 

342 

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

344 

345 Args: 

346 owner: The owner of the integrator 

347 input_signal: A callable accepting an object implementing the system 

348 object access protocol and providing the value of the derivative 

349 initial_condition: The initial condition of the integrator 

350 (default: ``None``) 

351 

352 Returns: 

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

354 """ 

355 

356 return SignalState(owner, 

357 shape=input_signal.shape, 

358 derivative_function=input_signal, 

359 initial_condition=initial_condition)