-
-
Notifications
You must be signed in to change notification settings - Fork 36
Expand file tree
/
Copy pathdifferentiator.py
More file actions
143 lines (108 loc) · 3.9 KB
/
differentiator.py
File metadata and controls
143 lines (108 loc) · 3.9 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
#########################################################################################
##
## DIFFERENTIATOR BLOCK
## (blocks/differentiator.py)
##
#########################################################################################
# IMPORTS ===============================================================================
import numpy as np
from ._block import Block
from ..optim.operator import DynamicOperator
# BLOCKS ================================================================================
class Differentiator(Block):
"""Differentiates the input signal.
Uses a first order transfer function with a pole at the origin which implements
a high pass filter. Supports vector input.
.. math::
H_\\mathrm{diff}(s) = \\frac{s}{1 + s / f_\\mathrm{max}}
The approximation holds for signals up to a frequency of approximately f_max.
Note
-----
Depending on `f_max`, the resulting system might become stiff or ill conditioned!
As a practical choice set `f_max` to 3x the highest expected signal frequency.
Note
----
Since this is an approximation of real differentiation, the approximation will not hold
if there are high frequency components present in the signal. For example if you have
discontinuities such as steps or squere waves.
Example
-------
The block is initialized like this:
.. code-block:: python
#cutoff at 1kHz
D = Differentiator(f_max=1e3)
Parameters
----------
f_max : float
highest expected signal frequency
Attributes
----------
op_dyn : DynamicOperator
internal dynamic operator for ODE component
op_alg : DynamicOperator
internal algebraic operator
"""
def __init__(self, f_max=1e2):
super().__init__()
#maximum frequency for differentiator approximation
self.f_max = f_max
#initial state for integration engine
self.initial_value = 0.0
self.op_dyn = DynamicOperator(
func=lambda x, u, t: self.f_max * (u - x),
jac_x=lambda x, u, t: -self.f_max*np.eye(len(u))
)
self.op_alg = DynamicOperator(
func=lambda x, u, t: self.f_max * (u - x),
jac_x=lambda x, u, t: -self.f_max*np.eye(len(u)),
jac_u=lambda x, u, t: self.f_max*np.eye(len(u)),
)
def __len__(self):
return 1 if self._active else 0
def update(self, t):
"""update system equation fixed point loop,
with convergence control
Parameters
----------
t : float
evaluation time
"""
x, u = self.engine.state, self.inputs.to_array()
y = self.op_alg(x, u, t)
self.outputs.update_from_array(y)
def solve(self, t, dt):
"""advance solution of implicit update equation
Parameters
----------
t : float
evaluation time
dt : float
integration timestep
Returns
-------
error : float
solver residual norm
"""
x, u = self.engine.state, self.inputs.to_array()
f, J = self.op_dyn(x, u, t), self.op_dyn.jac_x(x, u, t)
return self.engine.solve(f, J, dt)
def step(self, t, dt):
"""compute update step with integration engine
Parameters
----------
t : float
evaluation time
dt : float
integration timestep
Returns
-------
success : bool
step was successful
error : float
local truncation error from adaptive integrators
scale : float
timestep rescale from adaptive integrators
"""
x, u = self.engine.state, self.inputs.to_array()
f = self.op_dyn(x, u, t)
return self.engine.step(f, dt)