def regress_model_no_surface(self):
'''
regress the H matrix for DFL model
'''
omega = np.concatenate(
(self.X_minus.reshape(-1, self.X_minus.shape[-1]),
self.Eta_minus.reshape(-1, self.Eta_minus.shape[-1]),
self.U_minus.reshape(-1, self.U_minus.shape[-1])),
axis=1).T
Y = self.Eta_plus.reshape(-1, self.Eta_plus.shape[-1]).T
H_disc = lstsq(omega.T, Y.T, rcond=None)[0].T
self.H_disc_x = H_disc[:, :self.plant.n_x]
self.H_disc_eta = H_disc[:, self.plant.n_x:self.plant.n_x +
self.plant.n_eta]
self.H_disc_u = H_disc[:, self.plant.n_x +
self.plant.n_eta:self.plant.n_x +
self.plant.n_eta + self.plant.n_u]
(self.A_disc_x, self.B_disc_x, _, _, _) = cont2discrete(
(self.plant.A_cont_x, self.plant.B_cont_x, np.zeros(
self.plant.n_x), np.zeros(self.plant.n_u)), self.dt_data)
(_, self.A_disc_eta, _, _, _) = cont2discrete(
(self.plant.A_cont_x, self.plant.A_cont_eta,
np.zeros(self.plant.n_x), np.zeros(self.plant.n_u)), self.dt_data)
def __init__(self, VehicleParameters, EnvironmentParameters,TuningParameters,SimulationParameters):
# x-axis
A_x = np.zeros((2,2)); A_x[0,1] = 1;
B_x = np.zeros((2,1)); B_x[1] = -EnvironmentParameters.g;
C_x = np.zeros((2,2)); C_x[0,0] = 1; C_x[1,1] = 1;
D_x = np.zeros((2,1));
sys_x_d = signal.cont2discrete((A_x,B_x,C_x,D_x),TuningParameters.Ts,"zoh");
# y-axis
A_y = np.zeros((2,2)); A_y[0,1] = 1;
B_y = np.zeros((2,1)); B_y[1] = EnvironmentParameters.g;
C_y = np.zeros((2,2)); C_y[0,0] = 1; C_y[1,1] = 1;
D_y = np.zeros((2,1));
sys_y_d = signal.cont2discrete((A_y,B_y,C_y,D_y),TuningParameters.Ts,"zoh");
# z-axis
A_z = np.zeros((2,2)); A_z[0,1] = 1;
B_z = np.zeros((2,1)); B_z[1] = 1;
C_z = np.zeros((2,2)); C_z[0,0] = 1; C_z[1,1] = 1;
D_z = np.zeros((2,1));
sys_z_d = signal.cont2discrete((A_z,B_z,C_z,D_z),TuningParameters.Ts,"zoh");
self.VehicleParameters = VehicleParameters;
self.EnvironmentParameters = EnvironmentParameters;
self.TuningParameters = TuningParameters;
self.SimulationParameters = SimulationParameters;
self.SysX_d = sys_x_d;
self.SysY_d = sys_y_d;
self.SysZ_d = sys_z_d;
def get_filter(spec, filter_type='but', method='zoh'):
wp = 2*np.pi*spec['fp']
ws = 2*np.pi*spec['fs']
if method == 'bilinear':
wp = 2/spec['dt'] * np.arctan(wp * spec['dt']/2)
ws = 2/spec['dt'] * np.arctan(ws * spec['dt']/2)
if filter_type.lower() in ('butterworth'):
N, Wn = signal.buttord(wp, ws, spec['Amax'], spec['Amin'], analog=True)
z, p, k = signal.butter(N, Wn, output='zpk', btype='low', analog=True)
elif filter_type.lower() in ('cauer' + 'elliptic'):
N, Wn = signal.ellipord(wp, ws, spec['Amax'], spec['Amin'], analog=True)
z, p, k = signal.ellip(N, spec['Amax'], spec['Amin'], Wn, output='zpk', btype='low', analog=True)
if method == 'matched':
zd, pd, kd, dt = matched_method(z, p, k, spec['dt'])
kd *= 1 - (1 - 10 ** (-spec['Amax']/20))/2
else:
zd, pd, kd, dt = signal.cont2discrete((z,p,k), spec['dt'], method=method)
analog_system = (z,p,k)
discrete_system = (zd,pd,kd)
return analog_system, discrete_system
def getDiscrete(self, A, B):
"""
get ZOH approximation of A and B matrices
"""
(A_d, B_d, _, _, _) = cont2discrete((A, B, self.C, self.D), self.dt)
return A_d, B_d
def set_tf(self, input_: Signal, output: Signal, tf):
"""
Method to assign an output signal as a function of the input signal by applying a given transfer function.
The transfer function is discretized using a timestep of "dt" by applying the zero-order hold method.
:param input_: Input signal.
:param output: Output signal (should be an AnalogState that is not yet assigned)
:param tf: Tuple consisting of a list of numerator coefficients and a list of denominator coefficients. See
https://docs.scipy.org/doc/scipy-0.14.0/reference/generated/scipy.signal.cont2discrete.html for more details.
:return:
"""
# discretize transfer function
res = cont2discrete(tf, self.dt)
# get numerator and denominator coefficients
b = [+float(val) for val in res[0].flatten()]
a = [-float(val) for val in res[1].flatten()]
# create input and output histories
i_hist = self.make_history(input_, len(b))
o_hist = self.make_history(output, len(a))
# implement the filter
expr = sum_op([
coeff * var
for coeff, var in chain(zip(b, i_hist), zip(a[1:], o_hist))
])
# make the assignment
self.set_next_cycle(signal=output, expr=expr)
def get_filter(spec, filter_type='but', method='zoh'):
if filter_type.lower() in ('butterworth'):
N, Wn = signal.buttord(2*np.pi*spec['fp'], 2*np.pi*spec['fs'], spec['Amax'], spec['Amin'], analog=True)
z, p, k = signal.butter(N, Wn, output='zpk', btype='low', analog=True)
elif filter_type.lower() in ('cauer' + 'elliptic'):
N, Wn = signal.ellipord(2*np.pi*fp, 2*np.pi*spec['fs'], spec['Amax'], spec['Amin'], analog=True)
z, p, k = signal.ellip(N, spec['Amax'], spec['Amin'], Wn, output='zpk', btype='low', analog=True)
def matched_method(z, p, k, dt):
zd = np.exp(z*dt)
pd = np.exp(p*dt)
kd = k * np.abs(np.prod(1-pd)/np.prod(1-zd) * np.prod(z)/np.prod(p))
return zd, pd, kd, dt
if method == 'matched':
zd, pd, kd, dt = matched_method(z, p, k, spec['dt'])
kd *= 1 - (1 - 10 ** (-spec['Amax']/20))/2
else:
zd, pd, kd, dt = signal.cont2discrete((z,p,k), spec['dt'], method=method)
analog_system = (z,p,k)
discrete_system = (zd,pd,kd)
return analog_system, discrete_system
def __init__(self):
""" Linear aircraft model wrapped by OpenAI Gym template."""
# load saved model (csv format)
model_name = 'f18a_model'
model = np.genfromtxt(model_name, delimiter=',', skip_header=1)
self.labels = np.genfromtxt(model_name,
dtype=str,
delimiter=',',
max_rows=1)
self.labels = list(self.labels)
# organize matrices
self.n_states = model.shape[0]
self.n_controls = model.shape[
1] - self.n_states - 1 # last col is trimmed
self.A = model[:, :self.n_states]
self.B = model[:, self.n_states:-1]
self.label_states = self.labels[:self.n_states]
self.label_controls = self.labels[self.n_states:]
# trimmed states (x0)
self.x0 = model[:, -1].reshape(1, self.n_states)
# adding altitude (h)
self.n_states += 1
self.U1 = 1004.793
h_dot_a = np.array([[0, -self.U1, 0, self.U1, 0, 0, 0, 0, 0, 0]])
h_dot_b = np.array([[0, 0, 0]])
# augment old a and b
self.A = np.hstack((self.A, np.zeros((9, 1))))
self.A = np.vstack((self.A, h_dot_a))
self.B = np.vstack((self.B, h_dot_b))
# augment x0 and labels
self.label_states.append('$h$ (ft)')
h0 = 5000 # ft
self.x0 = np.column_stack((self.x0, h0))
# initialize C assuming full-state feedback and empty D
self.C = np.eye(self.n_states)
self.D = np.zeros_like(self.B)
# create system as discretize
self.dt = 1 / 50
self.dsys = signal.cont2discrete((self.A, self.B, self.C, self.D),
self.dt)
self.dA = self.dsys[0]
self.dB = self.dsys[1]
# ACTIONS
self.action_space = spaces.Box(low=-np.pi,
high=np.pi,
shape=(self.n_controls, ),
dtype=np.float32)
# STATES
self.observation_space = spaces.Box(low=-np.inf,
high=np.inf,
shape=(self.n_states, ),
dtype=np.float32)
def get_SS(
self, state
): #This appears to match what Mat has. Need to run his matlab code to be sure
v = state[3:6]
phi = state[6]
theta = state[7]
psi = state[8]
w = state[9:]
ct = np.cos(theta)
st = np.sin(theta)
cp = np.cos(phi)
sp = np.sin(phi)
A = np.zeros((12, 12))
dpd_dv = Rotation.from_euler('ZYX', [psi, theta, phi]).as_dcm()
A[:3, 3:6] = dpd_dv
dvd_dv = -skew(w)
dvd_dw = skew(v)
dvd_dang = np.array([[0, -self.g * ct, 0],
[self.g * ct * cp, -self.g * st * sp, 0],
[-self.g * ct * sp, -self.g * st * cp, 0]])
# A[3:6, 3:6] = dvd_dv #Mat isn't using this. Will try adding it later
A[3:6, 6:9] = dvd_dang
# A[3:6, 9:] = dvd_dw #Mat isn't using this. Will try adding it later
dangd_dw = np.array([[1.0, sp * st / ct, cp * st / ct], [0.0, cp, -sp],
[0.0, sp / ct, cp / ct]])
# A[6:9, 9:] = dangd_dw
A[6:9, 9:] = np.eye(
3
) #Mat is doing this not the line above. Will try switching it later
dwd_dw = np.linalg.inv(
self.J) @ (-skew(self.J @ w) @ (-np.eye(3)) + skew(-w) @ self.J)
# A[9:, 9:] = dwd_dw #Mat isn't using this. Will try adding it later
B = np.zeros((12, 4))
dvd_du = np.zeros((3, 4))
dvd_du[2, 0] = -1 / self.m
B[3:6] = dvd_du
dwd_du = np.zeros((3, 4))
dwd_du[:, 1:] = np.diag(
[1 / self.J[0, 0], 1 / self.J[1, 1], 1 / self.J[2, 2]])
B[9:] = dwd_du
C = np.eye(12) #Full state Feedback
D = np.zeros((12, 4))
sysd = ss.cont2discrete((A, B, C, D),
self.dt) #Get the discrete time system
Ad = sysd[0]
Bd = sysd[1]
# return A, B
return Ad, Bd
def linearized_model(state):
g = 9.8
m = 0.044
# linear continuous system
Ac = np.array([
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0]
])
Bc = np.array([
[0, 0, 0],
[0, 0, 0],
[0, 0, 0],
[g, 0, 0],
[0, - g, 0],
[0, 0, 1.0/m]
])
Cc = np.eye(Ac.shape[0]) # Full state observed
Dc = np.zeros((Bc.shape[0], Bc.shape[1]))
sysd = ss.cont2discrete((Ac, Bc, Cc, Dc), dt) # Get the discrete time system
Ad = sysd[0]
Bd = sysd[1]
return Ad, Bd
def __init__(
self,
input_size,
hidden_size,
memory_size,
memory_order,
A,
B,
trainable_scale=0., # how much to scale LR on A and B
dt=0.01,
discretization='zoh',
**kwargs):
super().__init__(input_size, hidden_size, memory_size, memory_order,
**kwargs)
C = np.ones((1, memory_order))
D = np.zeros((1, ))
dA, dB, _, _, _ = signal.cont2discrete((A, B, C, D),
dt=dt,
method=discretization)
dA = dA - np.eye(memory_order) # puts into form: x += Ax
self.trainable_scale = np.sqrt(trainable_scale)
if self.trainable_scale <= 0.:
self.register_buffer('A', torch.Tensor(dA))
self.register_buffer('B', torch.Tensor(dB))
else:
self.A = nn.Parameter(torch.Tensor(dA / self.trainable_scale),
requires_grad=True)
self.B = nn.Parameter(torch.Tensor(dB / self.trainable_scale),
requires_grad=True)
def generate_state_space(v, dt):
A1, B1 = benchmark_state_space(*benchmark_matrices(), v=v, g=9.80665)
A = np.zeros((5, 5))
B = np.zeros((5, 2))
A[1:, 1:] = A1
B[1:, :] = B1
p = benchmark_parameters()
A[0, 2] = v*np.cos(p['lambda'])/p['w']
A[0, 4] = p['c']*np.cos(p['lambda'])/p['w']
C = np.eye(5)
D = np.zeros((5, 2))
Ad, Bd, _, _, _ = sig.cont2discrete((A, B, C, D), dt, 'bilinear')
print('A <<')
print_matrix(A)
print('B <<')
print_matrix(B)
print('Ad <<')
print_matrix(Ad)
print('Bd <<')
print_matrix(Bd)
def initiate_model(self, configfile):
# Load parameters from configuration file
J = 0.01
b = 0.1
K = 0.01
K = 0.01
R = 1.
L = 0.5
# Time step
self.h = 0.1
self.nStates = 3
self.nMeasurements = 1
self.nControl = 1
# Continuous time DC model
A = np.array([[0., 1., 0.],[0., -b/J, K/J],[0., -K/L, -R/L]])
B = np.array([[0.],[0.],[1./L]])
C = np.array([[0.,1.,0.]])
D = np.array([[0.]])
# Discretize system using zero order hold
dsys = cont2discrete((A, B, C, D), self.h, method='zoh', alpha=None)
self.Ad = dsys[0]
self.Bd = dsys[1]
self.Cd = dsys[2]
self.Dd = dsys[3]
def make_step(self, shape_in, shape_out, dt, rng, y0=None,
dtype=np.float64, method='zoh'):
"""Produces the function for filtering across one time-step."""
assert shape_in == shape_out
output = np.zeros(shape_out)
if y0 is not None:
output[...] = y0
if y0 is None or not np.allclose(y0, 0):
warnings.warn(
"y0 (%s!=0) does not properly initialize the system; see "
"Nengo issue #1124." % y0, UserWarning)
if self.analog:
# TODO: equivalent to cont2discrete in discrete.py, but repeated
# here to avoid circular dependency.
A, B, C, D, _ = cont2discrete(self.ss, dt, method=method)
sys = LinearSystem((A, B, C, D), analog=False)
else:
sys = self
if not sys.has_passthrough:
# This makes our system behave like it does in Nengo
sys *= z # discrete shift of the system to remove delay
else:
warnings.warn(
"Synapse (%s) has extra delay due to passthrough "
"(https://github.com/nengo/nengo/issues/938)." % sys)
return _CanonicalStep(sys, output, y0=y0, dtype=dtype)
def time_update(self, k, x_hat, u_k):
res = self.nonlin.simulate(state=x_hat,
start_time=k * self.Ts,
final_time=(k + 1) * self.Ts,
input=u_k,
append=True)
# Linearize
Ak, Bk, Ck, Dk = self.nonlin.sim.get_state_space_representation()
# Discretize
(Ak, Bk, Ck, Dk, dt) = sig.cont2discrete(
(Ak.toarray(), Bk.toarray(), Ck.toarray(), Dk.toarray()), self.Ts)
self.Ak = Ak
# Remove ignored states
self.Ak = np.delete(self.Ak, (self.ignored_states.values()), axis=0)
self.Ak = np.delete(self.Ak, (self.ignored_states.values()), axis=1)
# Turn into numpy matrix, for matrix multiplication with *
self.Ak = np.asmatrix(self.Ak)
# Obtain state values
x_hat = []
for state in self.states:
x_hat.append(res[state][-1])
# Transpose, to obtain a state column vector
self.x_hat = np.array([x_hat]).T
# Calculate measurement column vector
self.y_hat = self.Ck * self.x_hat
# Obtain new error covariance
self.P = self.Ak * self.P * self.Ak.T + self.Q
def tf(self):
analog = EgoStengelOriginal()
analog.passband = self.prewarped_passband
b, a, _ = cont2discrete(analog.tf, self.dt, method="bilinear")
# An idiosyncracy of cont2discrete:
b = b.flatten()
return b, a
def simulate(r_series):
Num = len(r_series)
#GA用パラメータ部
K_p = 1.0
K_d = 0.5
A = np.array([[0, 1], [-K_p, -0.5 - K_d]])
B = np.array([[0], [1.0]])
C = np.array([[-K_p, -K_d]])
D = np.array([K_p])
Ts = 0.05
c2d = sg.cont2discrete((A, B, C, D), dt=Ts)
A_d, B_d, C_d, D_d = c2d[0], c2d[1].reshape(2), c2d[2], c2d[3]
#GA用パラメータ部終了
#保存用ログ確保
x_series = np.zeros((Num, 2), dtype=np.float64)
u_series = np.zeros((Num, 1), dtype=np.float64)
for i in range(Num - 1):
x = A_d @ x_series[i] + B_d * r_series[i]
u = C_d @ x_series[i] + D_d * r_series[i]
x_series[i + 1] = x
u_series[i + 1] = u
return x_series, u_series
def __init__(self, param):
self._set_param(param)
# Create continouous time linearized model
Ac = np.zeros([12, 12])
Ac[0:3, 3:6] = np.diag([1., 1., 1.])
Ac[3:6, 3:6] = -np.diag(self.A) / self.m
Ac[6:9, 9:12] = np.diag([1., 1., 1.])
Bc = np.zeros([12, 4])
Bc[5, :] = self.k
Bc[9:12, 0:4] = np.array(
[[0., -self.k * self.l / self.Ixx, 0., self.k * self.l / self.Ixx],
[-self.k * self.l / self.Iyy, 0., self.k * self.l / self.Ixx, 0.],
[
-self.b / self.Izz, self.b / self.Izz, -self.b / self.Izz,
self.b / self.Izz
]])
Cc = np.zeros([7, 12])
Cc[0:7, 2:9] = np.diag(np.ones([1, 7])[0])
Dc = np.zeros([7, 4])
# Discretize model
discreteSystem = cont2discrete((Ac, Bc, Cc, Dc),
self.Ts,
method='zoh',
alpha=None)
# Set discrete matrices
self.Ad = discreteSystem[0]
self.Bd = discreteSystem[1]
self.Cd = discreteSystem[2]
def __init__(self, param):
self._set_param(param)
# Create continouous time linearized model
Ac = np.zeros([12,12])
Ac[0:3,3:6] = np.diag([1.,1.,1.])
Ac[3:6,3:6] = -np.diag(self.A)/self.m
Ac[6:9,9:12] = np.diag([1.,1.,1.])
Bc = np.zeros([12,4])
Bc[5,:] =self.k
Bc[9:12,0:4] = np.array([[0., -self.k*self.l/self.Ixx, 0., self.k*self.l/self.Ixx],
[-self.k*self.l/self.Iyy, 0., self.k*self.l/self.Ixx, 0.],
[-self.b/self.Izz, self.b/self.Izz, -self.b/self.Izz, self.b/self.Izz]])
Cc = np.zeros([7,12])
Cc[0:7,2:9] = np.diag(np.ones([1,7])[0])
Dc = np.zeros([7,4])
# Discretize model
discreteSystem = cont2discrete((Ac,Bc,Cc,Dc), self.Ts, method='zoh', alpha=None)
# Set discrete matrices
self.Ad = discreteSystem[0]
self.Bd = discreteSystem[1]
self.Cd = discreteSystem[2]
def control_systems(request):
ct_sys, ref = request.param
Ac, Bc, Cc = ct_sys.data
Dc = np.zeros((Cc.shape[0], 1))
Q = np.eye(Ac.shape[0])
R = np.eye(Bc.shape[1] if len(Bc.shape) > 1 else 1)
Sc = linalg.solve_continuous_are(Ac, Bc.reshape(-1, 1), Q, R,)
Kc = linalg.solve(R, Bc.T @ Sc).reshape(1, -1)
ct_ctr = LTISystem(Kc)
evals = np.sort(np.abs(
linalg.eig(Ac, left=False, right=False, check_finite=False)
))
dT = 1/(2*evals[-1])
Tsim = (8/np.min(evals[~np.isclose(evals, 0)])
if np.sum(np.isclose(evals[np.nonzero(evals)], 0)) > 0
else 8
)
dt_data = signal.cont2discrete((Ac, Bc.reshape(-1, 1), Cc, Dc), dT)
Ad, Bd, Cd, Dd = dt_data[:-1]
Sd = linalg.solve_discrete_are(Ad, Bd.reshape(-1, 1), Q, R,)
Kd = linalg.solve(Bd.T @ Sd @ Bd + R, Bd.T @ Sd @ Ad)
dt_sys = LTISystem(Ad, Bd, dt=dT)
dt_sys.initial_condition = ct_sys.initial_condition
dt_ctr = LTISystem(Kd, dt=dT)
yield ct_sys, ct_ctr, dt_sys, dt_ctr, ref, Tsim
def discretise(self, dt, method="zoh", alpha=None):
"""Transform a continuous-time system into a discrete-time one.
Parameters
----------
dt : float
the discretisation time step
method : str, optional
discretisation method, refer to the documentation for
:func:`scipy.signal.cont2discrete`
alpha : float within [0, 1], optional
weighting parameter, refer to the documentation for
:func:`scipy.signal.cont2discrete`
Returns
-------
controlboros.StateSpaceBuilder
intermediate builder object
"""
self._a, self._b, self._c, self._d, _ = signal.cont2discrete(
(self._a, self._b, self._c, self._d),
dt,
method=method,
alpha=alpha,
)
self._discrete = True
return self
def simulate(self, r_series):
#実際の状態方程式におけるシミュレーション
#印加される目標値に対して状態と入力を出力
Num = len(r_series)
#パラメータ部開始
K_p = 1.0
K_d = 0.5
x_series = np.zeros((Num, 2), dtype=np.float64)
u_series = np.zeros((Num, 1), dtype=np.float64)
A = np.array([[0, 1], [-K_p, -0.5 - K_d]])
B = np.array([[0], [1.0]])
C = np.array([[-K_p, -K_d]])
D = np.array([K_p])
Ts = 0.05
c2d = sg.cont2discrete((A, B, C, D), dt=Ts)
A_d, B_d, C_d, D_d = c2d[0], c2d[1].reshape(2), c2d[2], c2d[3]
#パラメータ部終了
#更新式
for i in range(Num - 1):
x = A_d @ x_series[i] + B_d * r_series[i]
u = C_d @ x_series[i] + D_d * r_series[i]
x_series[i + 1] = x
u_series[i + 1] = u
return x_series, u_series
def linearize(self):
"""Return the discretized, scaled, linearized system.
Returns
-------
Ad : ndarray
The discrete-time state matrix.
Bd : ndarray
The discrete-time action matrix.
"""
m = self.pendulum_mass
M = self.cart_mass
L = self.length
b = self.rot_friction
g = self.gravity
A = np.array(
[[0, 0, 1, 0], [0, 0, 0, 1], [0, g * m / M, 0, -b / (M * L)],
[0, g * (m + M) / (L * M), 0, -b * (m + M) / (m * M * L**2)]],
dtype=config.np_dtype)
B = np.array([0, 0, 1 / M, 1 / (M * L)]).reshape((-1, self.action_dim))
if self.normalization is not None:
Tx, Tu = map(np.diag, self.normalization)
Tx_inv, Tu_inv = map(np.diag, self.inv_norm)
A = np.linalg.multi_dot((Tx_inv, A, Tx))
B = np.linalg.multi_dot((Tx_inv, B, Tu))
Ad, Bd, _, _, _ = signal.cont2discrete((A, B, 0, 0),
self.dt,
method='zoh')
return Ad, Bd
def test_ctle2(fz, fp1, npts, order, gbw, dtmax, err_lim):
# normalize frequencies
fz = fz*dtmax
fp1 = fp1*dtmax
gbw = gbw*dtmax
# read in data
my_data = np.genfromtxt(DATA_FILE, delimiter=',', skip_header=1)
t_resp = my_data[:, 1] - my_data[0, 1]
v_resp = my_data[:, 2]
# find timestep of oversampled data
tover = np.median(np.diff(t_resp))
assert np.all(np.isclose(np.diff(t_resp), tover))
print(f'tover: {tover*1e12:0.3f} ps')
# build interpolator for data
my_interp = interp1d(t_resp, v_resp)
svec = np.linspace(0, 1, npts)
# find state-space representation of the CTLE
A, B, C, D = calc_ctle_abcd(fz=fz, fp1=fp1, gbw=gbw)
# calculate response using spline method
# the list of timesteps used is one that was found to be particularly bad for the emulator
W = calc_interp_w(npts=npts, order=order)
ctle = SplineLDS(A=A, B=B, C=C, D=D, W=W)
x = np.zeros((A.shape[0],), dtype=float)
t = 0
dtlist = [0.960, 0.080, 0.960, 0.960, 0.080, 0.960, 0.960, 0.080, 0.960, 0.960,
0.080, 0.960, 0.960, 0.080, 0.960, 0.960, 0.080, 0.960, 0.960, 0.080,
0.960, 0.960, 0.080, 0.960, 0.960, 0.080, 0.960, 0.960, 0.080, 0.960,
0.960, 0.080, 0.960, 0.960, 0.080, 0.960, 0.960, 0.080, 0.960, 0.960,
0.080, 0.960, 0.960, 0.080, 0.960, 0.960, 0.080]
tlist = []
ylist = []
for dt in dtlist:
tlist.append(t)
x, y = ctle.calc_update(xo=x, inpt=my_interp(t+svec*dtmax), dt=dt)
ylist.append(y)
t += dt*dtmax
# calculate measured values
y_meas = interp_emu_res(tlist, ylist, dtmax, t_resp, order, npts)
# find expected response of the CTLE
num, den = calc_ctle_num_den(fz=fz, fp1=fp1, gbw=gbw)
b, a, _ = cont2discrete((num, den), dt=tover/dtmax)
y_expt = lfilter(b[0], a, v_resp)
# uncomment to plot results
# import matplotlib.pyplot as plt
# plt.plot(t_resp, y_expt)
# plt.plot(t_resp, y_meas)
# plt.show()
# calculate error
rms_err = np.sqrt(np.mean((y_expt-y_meas)**2))
print('rms_err:', rms_err)
assert rms_err < err_lim
def generate_state_space(v, dt):
A1, B1 = benchmark_state_space(*benchmark_matrices(), v=v, g=9.80665)
A = np.zeros((5, 5))
B = np.zeros((5, 2))
A[1:, 1:] = A1
B[1:, :] = B1
p = benchmark_parameters()
A[0, 2] = v * np.cos(p['lambda']) / p['w']
A[0, 4] = p['c'] * np.cos(p['lambda']) / p['w']
C = np.eye(5)
D = np.zeros((5, 2))
Ad, Bd, _, _, _ = sig.cont2discrete((A, B, C, D), dt, 'bilinear')
print('A <<')
print_matrix(A)
print('B <<')
print_matrix(B)
print('Ad <<')
print_matrix(Ad)
print('Bd <<')
print_matrix(Bd)
def generate_hybrid_model(self, xi_order):
U = np.concatenate((self.X_minus.reshape(-1, self.X_minus.shape[-1]),
self.U_minus.reshape(-1, self.U_minus.shape[-1])),
axis=1).T
Y_temp = self.Eta_minus.reshape(-1, self.Eta_minus.shape[-1]).T
Y = self.plant.P.dot(Y_temp)
if len(Y.shape) == 1:
Y = Y.T
Y = np.expand_dims(Y, axis=0)
(A_disc_x, B_disc_x, _, _, _) = cont2discrete(
(self.plant.A_cont_x, self.plant.B_cont_x, np.zeros(
self.plant.n_x), np.zeros(self.plant.n_u)), self.dt_data)
(_, A_disc_eta_hybrid, _, _, _) = cont2discrete(
(self.plant.A_cont_x, self.plant.A_cont_eta_hybrid,
np.zeros(self.plant.n_x), np.zeros(self.plant.n_eta)),
self.dt_data)
method = 'N4SID'
sys_id = system_identification(
Y, U, method, SS_D_required=True,
SS_fixed_order=int(xi_order)) #, IC='AICc')#
# SS_fixed_order = self.plant.N_eta,
A_til, B_til, C_til, D_til = sys_id.A, sys_id.B, sys_id.C, sys_id.D
B_til_1 = B_til[:, :self.plant.n_x]
B_til_2 = B_til[:, self.plant.n_x:]
D_til_1 = D_til[:, :self.plant.n_x]
D_til_2 = D_til[:, self.plant.n_x:]
A1 = A_disc_x + A_disc_eta_hybrid.dot(D_til_1)
A2 = A_disc_eta_hybrid.dot(C_til)
B1 = B_disc_x + A_disc_eta_hybrid.dot(D_til_2)
self.A_disc_hybrid_full = np.block([[A1, A2], [B_til_1, A_til]])
self.B_disc_hybrid_full = np.block([[B1], [B_til_2]])
self.C_til = C_til
self.D_til_1 = D_til_1
self.D_til_2 = D_til_2
def c2d(A, B, C, D, dt):
"""Convert continuous state system to discrete time.
Converts a set of continuous state space system matrices to their
discrete counterpart.
Parameters
----------
A, B, C, D : float arrays
State space system matrices
dt : float
Time step of discrete system
Returns
-------
Ad, Bd, C, D : float arrays
Discrete state space system matrices
Examples
--------
>>> import numpy as np
>>> np.set_printoptions(precision=4, suppress=True)
>>> import vibrationtesting as vt
>>> A1 = np.array([[ 0., 0. , 1. , 0. ]])
>>> A2 = np.array([[ 0., 0. , 0. , 1. ]])
>>> A3 = np.array([[-1.4, 1.2, -0.0058, 0.0014]])
>>> A4 = np.array([[ 0.8, -1.4, 0.0016, -0.0038]])
>>> A = np.array([[ 0., 0. , 1. , 0. ],
... [ 0., 0. , 0. , 1. ],
... [-1.4, 1.2, -0.0058, 0.0014],
... [ 0.8, -1.4, 0.0016, -0.0038]])
>>> B = np.array([[ 0.],
... [ 0.],
... [ 0.],
... [ 1.]])
>>> C = np.array([[-1.4, 1.2, -0.0058, 0.0014]])
>>> D = np.array([[-0.2]])
>>> Ad, Bd, *_ = vt.c2d(A, B, C, D, 0.01)
>>> print(Ad)
[[ 0.9999 0.0001 0.01 0. ]
[ 0. 0.9999 0. 0.01 ]
[-0.014 0.012 0.9999 0.0001]
[ 0.008 -0.014 0.0001 0.9999]]
>>> print(Bd)
[[ 0. ]
[ 0. ]
[ 0. ]
[ 0.01]]
Notes
-----
.. note:: Zero-order hold solution
.. seealso:: :func:`d2c`.
"""
Ad, Bd, _, _, _ = sig.cont2discrete((A, B, C, D), dt)
# Ad = la.expm(A * dt)
# Bd = la.solve(A, (Ad - np.eye(A.shape[0]))) @ B
return Ad, Bd, C, D
def main():
# Define state space
m = VEHICLE_MASS
b = LINEAR_DRAG_COEFFICIENT
A = np.array([[0, 1], [0, -b / m]])
B = np.array([[0], [1 / m]])
C = np.array([[1, 0]])
D = np.array([[0]])
Q = np.array([[MEASUREMENT_NOISE_COVARIANCE]])
R = np.array([[PROCESS_NOISE_COVARIANCE_POSITION, 0],
[0, PROCESS_NOISE_COVARIANCE_VELOCITY]])
state_space = cont2discrete((A, B, C, D), dt=SAMPLE_PERIOD)
A = state_space[0]
B = state_space[1]
C = state_space[2]
D = state_space[3]
Atranspose = A.transpose()
Ctranspose = C.transpose()
# Initial beliefs
mean_belief = np.array([[0], [0]])
variance_belief = np.array([[1, 0], [0, 1]])
# Initial conditions
xt_current = 0
vt_current = 0
# Initialize histories for plotting
all_xt = [xt_current]
all_vt = [vt_current]
all_mean_belief = [mean_belief]
all_variance_belief = [variance_belief]
all_kt = []
# Execute kalman filter for 50 seconds
time_steps = int(50 / SAMPLE_PERIOD)
for time_step in range(time_steps):
# Calculate input
ut = thrust((time_step + 1) * SAMPLE_PERIOD)
# Update true state
state = np.array([[xt_current], [vt_current]])
state_plus_one = dot(A, state) + dot(B, ut) + process_noise(R)
xt_current = state_plus_one[0][0]
vt_current = state_plus_one[1][0]
# Update sensor measurement of state
zt = xt_current + measurement_noise(Q)
# Calculate beliefs
mean_belief, variance_belief, kt = kalman_filter(
mean_belief, variance_belief, ut, zt, A, B, C, Atranspose,
Ctranspose, R, Q)
# Record outputs for plotting
all_xt.append(xt_current)
all_vt.append(vt_current)
all_mean_belief.append(mean_belief)
all_variance_belief.append(variance_belief)
all_kt.append(kt)
plot_everything(all_xt, all_vt, all_mean_belief, all_variance_belief,
all_kt)
def run_simulation(t_resp, v_resp):
# find timestep of oversampled data
tover = np.median(np.diff(t_resp))
assert np.all(np.isclose(np.diff(t_resp), tover))
print(f'tover: {tover*1e12:0.3f} ps')
# CLTE1
nd1 = calc_ctle_num_den(fz=ctle1_fz * dtmax,
fp1=ctle1_fp * dtmax,
gbw=gbw * dtmax)
b1, a1, _ = cont2discrete(nd1, dt=tover / dtmax)
ctle1_out = lfilter(b1[0], a1, v_resp)
# NL1
nl_vsat = calc_tanh_vsat(nl_dB, 'dB', veval=nl_veval)
nl_func = lambda v: tanhsat(v, nl_vsat)
nl1_out = nl_func(ctle1_out)
# CTLE2
nd2 = calc_ctle_num_den(fz=ctle2_fz * dtmax,
fp1=ctle2_fp * dtmax,
gbw=gbw * dtmax)
b2, a2, _ = cont2discrete(nd2, dt=tover / dtmax)
ctle2_out = lfilter(b2[0], a2, nl1_out)
# NL2
nl2_out = nl_func(ctle2_out)
# CTLE3
nd3 = calc_ctle_num_den(fz=ctle3_fz * dtmax,
fp1=ctle3_fp * dtmax,
gbw=gbw * dtmax)
b3, a3, _ = cont2discrete(nd3, dt=tover / dtmax)
ctle3_out = lfilter(b3[0], a3, nl2_out)
# NL3
nl3_out = nl_func(ctle3_out)
return {
'ctle1_out': ctle1_out,
'nl1_out': nl1_out,
'ctle2_out': ctle2_out,
'nl2_out': nl2_out,
'ctle3_out': ctle3_out,
'nl3_out': nl3_out
}
def f(x,u,eta=None): # Continuous-Time state-space matrices A_x = np.array([[0,1],[0,0]]) A_eta = np.array([[0,0],[-1/m,-1/m]]) B_x = np.array([[0],[1/m]]) # Convert state-space matrices to discrete-time _ , A_eta, _, _, _ = cont2discrete((A_x, A_eta, np.zeros(l_x), np.zeros(l_eta)), dt) A_x, B_x , _, _, _ = cont2discrete((A_x, B_x , np.zeros(l_x), np.zeros(l_u )), dt) # Nonlinear elements if eta is None: eta = g(x) return np.matmul(A_x , x) + \ np.matmul(A_eta, eta) + \ np.matmul(B_x , u)
def discrete_dynamics(self):
"""
Discrete state-space model
"""
self.discrete = signal.cont2discrete(self.continuous,
self.params['dt'],
method='zoh')
def cont2shift(self, dt):
if not self.is_continuous():
msg = 'System must be continuous to call this function.'
raise ValueError(msg)
tup = signal.cont2discrete(self.cofs, dt, method='bilinear')
return StateSpace(tup[0:4], dt=dt)
def loop(self):
# predict
self.getFB()
(self.Fd, self.Bd, self.H, self.D, self.dT) = cont2discrete(
(self.F, self.B, self.H, self.D), self.dT)
self.x_pre = self.fx(self.x_est, self.u)
self.P_pre = multi_dot([self.Fd, self.P_est, self.Fd.T]) + self.Q
# calculate Kalman Gain, estimate
if self.ir_velocity_tf is True:
print 'IR on'
self.K = multi_dot([
self.P_pre, self.H[0:9, :].T,
inv(
multi_dot([self.H[0:9, :], self.P_pre, self.H[0:9, :].T]) +
self.R[0:9, 0:9])
])
self.hx = self.gethx(self.x_pre)
self.x_est = self.x_pre + np.dot(self.K,
self.z[0:9, :] - self.hx[0:9, :])
#self.P_est = np.dot(np.eye(9)-np.dot(self.K, self.H[0:9, :]), self.P_pre)
self.P_est = multi_dot([
np.eye(9) - np.dot(self.K, self.H[0:9, :]), self.P_pre,
(np.eye(9) - np.dot(self.K, self.H[0:9, :])).T
]) + multi_dot([self.K, self.R[0:9, 0:9], self.K.T])
elif self.ir_pose_tf is True:
print 'IR pose on'
self.K = multi_dot([
self.P_pre, self.H[3:9, :].T,
inv(
multi_dot([self.H[3:9, :], self.P_pre, self.H[3:9, :].T]) +
self.R[3:9, 3:9])
])
self.hx = self.gethx(self.x_pre)
self.x_est = self.x_pre + np.dot(self.K,
self.z[3:9, :] - self.hx[3:9, :])
#self.P_est = np.dot(np.eye(9)-np.dot(self.K, self.H[3:9, :]), self.P_pre)
self.P_est = multi_dot([
np.eye(9) - np.dot(self.K, self.H[3:9, :]), self.P_pre,
(np.eye(9) - np.dot(self.K, self.H[3:9, :])).T
]) + multi_dot([self.K, self.R[3:9, 3:9], self.K.T])
else:
print 'IMU'
self.x_est = self.x_pre
self.P_est = self.P_pre
self.limitAngle()
self.setState()
self.pub_state.publish(self.state)
self.pub_position.publish(self.position)
self.pub_attitude.publish(self.attitude)
self.pub_linear_velocity.publish(self.linear_velocity)
self.pub_inertial_acceleration.publish(self.inertial_acceleration)
self.ir_pose_tf = False
self.ir_velocity_tf = False
self.r.sleep()
def c2d(A, B, C, D, dt):
"""returns Ad, Bd, C, D
Converts a set of digital state space system matrices to their continuous counterpart.
Simply calls scipy.signal.cont2discrete
"""
Ad, Bd, _, _, _ = sig.cont2discrete((A, B, C, D), dt)
Ad = la.expm(A * dt)
Bd = la.solve(A, (Ad - sp.eye(A.shape[0]))) @ B
return Ad, Bd, C, D
def sample_system(sysc, Ts, method='matched'):
# TODO: add docstring
# Make sure we have a continuous time system
if not isctime(sysc):
raise ValueError("First argument must be continuous time system")
# TODO: impelement MIMO version
if (sysc.inputs != 1 or sysc.outputs != 1):
raise NotImplementedError("MIMO implementation not available")
# If we are passed a state space system, convert to transfer function first
if isinstance(sysc, StateSpace):
warn("sample_system: converting to transfer function")
sysc = _convertToTransferFunction(sysc)
# Decide what to do based on the methods available
if method == 'matched':
sysd = _c2dmatched(sysc, Ts)
elif method == 'tustin':
sys = [sysc.num[0][0], sysc.den[0][0]]
scipySysD = cont2discrete(sys, Ts, method='bilinear')
sysd = TransferFunction(scipySysD[0][0], scipySysD[1], dt)
elif method == 'zoh':
sys = [sysc.num[0][0], sysc.den[0][0]]
scipySysD = cont2discrete(sys, Ts, method='zoh')
sysd = TransferFunction(scipySysD[0][0],scipySysD[1], dt)
elif method == 'foh' or method == 'impulse':
raise ValueError("Method not developed yet")
else:
raise ValueError("Invalid discretization method: %s" % method)
# TODO: Convert back into the input form
# Set sampling time
return sysd
def __init__(self, model, eta_c=0.9, eta_d=0.9, Tleak=96, ts=15):
"""
Create an instance of a generic buffered production model. Here
eta_c and eta_d are the charging and discharging efficiencies of
the battery, respectively, Tleak is time constant (in hours) of
the charge leakage, and ts is the sampling time (in minutes).
"""
Act = np.array([[-1.0/(Tleak*3600), 0], [0, 0]])
Bct = np.array([[eta_c, -1.0/eta_d], [0, 1]])
Cct = np.array([[0, 0]])
Dct = np.array([[0, 0]])
(A, B, C, D, dt) = cont2discrete(
(Act, Bct, Cct, Dct), ts*60, method='zoh')
super(GenericBufferedProduction, self).__init__(model, A, B)
def sample(self, Ts, method='zoh', alpha=None):
"""Convert a continuous-time system to discrete time
Creates a discrete-time system from a continuous-time system by
sampling. Multiple methods of conversion are supported.
Parameters
----------
Ts : float
Sampling period
method : {"gbt", "bilinear", "euler", "backward_diff", "zoh", "matched"}
Which method to use:
* gbt: generalized bilinear transformation
* bilinear: Tustin's approximation ("gbt" with alpha=0.5)
* euler: Euler (or forward differencing) method ("gbt" with alpha=0)
* backward_diff: Backwards differencing ("gbt" with alpha=1.0)
* zoh: zero-order hold (default)
alpha : float within [0, 1]
The generalized bilinear transformation weighting parameter, which
should only be specified with method="gbt", and is ignored otherwise
Returns
-------
sysd : StateSpace system
Discrete time system, with sampling rate Ts
Notes
-----
1. Available only for SISO systems
2. Uses the command `cont2discrete` from `scipy.signal`
Examples
--------
>>> sys = TransferFunction(1, [1,1])
>>> sysd = sys.sample(0.5, method='bilinear')
"""
if not self.isctime():
raise ValueError("System must be continuous time system")
if not self.issiso():
raise NotImplementedError("MIMO implementation not available")
if method == "matched":
return _c2dmatched(self, Ts)
sys = (self.num[0][0], self.den[0][0])
numd, dend, dt = cont2discrete(sys, Ts, method, alpha)
return TransferFunction(numd[0,:], dend, dt)
def __init__(self, model, eta_c=0.95, eta_d=0.95, Tleak=96, ts=15):
"""
Create an instance of a QuadraticUtilityWithBattery model. Here
eta_c and eta_d are the battery's charging and discharging
efficiencies, respectively, Tleak is time constant (in hours) of
the charge leakage, and ts is the sampling time (in minutes).
"""
Act = np.array([[-1.0/(Tleak*3600)]])
# Act = np.expand_dims(Act, axis=0)
Bct = np.array([[eta_c, -1.0/eta_d, 0]])
Cct = np.array([[0]])
Dct = np.array([[0, 0, 0]])
(A, B, C, D, dt) = cont2discrete(
(Act, Bct, Cct, Dct), ts*60, method='zoh')
B = np.array([[eta_c, -1.0/eta_d, 0]])
super(QuadraticUtilityWithBattery, self).__init__(model, A, B)
def sample(self, Ts, method='zoh', alpha=None):
"""Convert a continuous time system to discrete time
Creates a discrete-time system from a continuous-time system by
sampling. Multiple methods of conversion are supported.
Parameters
----------
Ts : float
Sampling period
method : {"gbt", "bilinear", "euler", "backward_diff", "zoh"}
Which method to use:
* gbt: generalized bilinear transformation
* bilinear: Tustin's approximation ("gbt" with alpha=0.5)
* euler: Euler (or forward differencing) method ("gbt" with
alpha=0)
* backward_diff: Backwards differencing ("gbt" with alpha=1.0)
* zoh: zero-order hold (default)
alpha : float within [0, 1]
The generalized bilinear transformation weighting parameter, which
should only be specified with method="gbt", and is ignored
otherwise
Returns
-------
sysd : StateSpace
Discrete time system, with sampling rate Ts
Notes
-----
Uses the command 'cont2discrete' from scipy.signal
Examples
--------
>>> sys = StateSpace(0, 1, 1, 0)
>>> sysd = sys.sample(0.5, method='bilinear')
"""
if not self.isctime():
raise ValueError("System must be continuous time system")
sys = (self.A, self.B, self.C, self.D)
Ad, Bd, C, D, dt = cont2discrete(sys, Ts, method, alpha)
return StateSpace(Ad, Bd, C, D, dt)
def _first_order_markov(self, tau, sigma, dt):
"""
Forms a first order Markov process model of the form:
dx(t)/dt = (-1/tau) x(t) + w(t)
where w(t) ~ N(0,Qw) is the driving white noise and
Q = sigma * sigma is the steady state variance of x(t)
Parameters
----------
tau: time constant [sec]
sigma: steady state (continuous time) standard deviation of x(t)
dt: discrete system time step
Returns
-------
Qw_d: the discrete-time equivalent covariance for the white-noise
driving process w(t)
A_d, B_d: the discrete time system specification for simulating the
Markove process:
x(k+1) = A_d * x(k) + B_d * w(k)
where w(k) ~ N(0,Qw_d) and A_d, B_d are scalars
all returned values are scalars
"""
a = np.matrix(-1.0/tau)
b, c, d = np.matrix(1.0), np.matrix(1.0), np.matrix(0.0)
# Driving Noise White Power Spectral Density
# This defines the relationship between the steady state variance
# and the variance of the driving white noise.
Qw = np.matrix(2.0 * sigma * sigma / tau)
# Determine the discrete-time equivalent process noise
# for the driving white process
Qw_d = disrw(a, b, dt, Qw, order=5)
# Convert continuous time to discrete time system for Markov process
SS_dis = signal.cont2discrete((a, b, c, d), dt)
A_d, B_d, C_d, D_d, Ts = SS_dis
return Qw_d.item(), A_d.item(), B_d.item()
def make_step(self, shape_in, shape_out, dt, rng, y0=None,
dtype=np.float64, method='zoh'):
assert shape_in == shape_out
output = np.zeros(shape_out)
if self.analog:
# Note: equivalent to cont2discrete in discrete.py, but repeated
# here to avoid circular dependency.
A, B, C, D, _ = cont2discrete(self.ss, dt, method=method)
sys = LinearSystem((A, B, C, D), analog=False)
else:
sys = self
if not sys.has_passthrough:
# This makes our system behave like it does in Nengo
sys *= z # discrete shift of the system to remove delay
else:
warnings.warn("Synapse (%s) has extra delay due to passthrough "
"(https://github.com/nengo/nengo/issues/938)" % sys)
return _DigitalStep(sys, output, y0=y0, dtype=dtype)
def time_update(self, k, x_hat, u_k):
res = self.nonlin.simulate(
state=x_hat,
start_time=k*self.Ts,
final_time=(k+1)*self.Ts,
input=u_k,
append=True
)
# Linearize
Ak, Bk, Ck, Dk = self.nonlin.sim.get_state_space_representation()
# Discretize
(Ak, Bk, Ck, Dk, dt) = sig.cont2discrete((
Ak.toarray(), Bk.toarray(), Ck.toarray(), Dk.toarray()
), self.Ts)
self.Ak = Ak
# Remove ignored states
self.Ak = np.delete(self.Ak, (self.ignored_states.values()), axis=0)
self.Ak = np.delete(self.Ak, (self.ignored_states.values()), axis=1)
# Turn into numpy matrix, for matrix multiplication with *
self.Ak = np.asmatrix(self.Ak)
# Obtain state values
x_hat = []
for state in self.states:
x_hat.append(res[state][-1])
# Transpose, to obtain a state column vector
self.x_hat = np.array([x_hat]).T
# Calculate measurement column vector
self.y_hat = self.Ck * self.x_hat
# Obtain new error covariance
self.P = self.Ak * self.P * self.Ak.T + self.Q
def handle_reference_data(self, msg):
# TODO include discretised quadcopter dynamics and publish measurements
omega = np.array(msg.data)
x = self.x
Ts = self.timestep
g = self.g
k = self.k
A = self.A
I = self.I
l = self.l
b = self.b
m = self.m
# Equations from Teppo Luukkonen, Modelling and control of quadcopter
# Extracs the euler angle states and positional derivatives
phi, theta, psi, phidot, thetadot, psidot = np.reshape(x[6:12,0:1],[1,6])[0]
# eq. (7)
T = k * sum(omega ** 2)
# Define the C-matrix, see eq. (19)
Ixx, Iyy, Izz = I
Sphi = sin(phi)
Stheta = sin(theta)
Spsi = sin(psi);
Cphi = cos(phi)
Ctheta = cos(theta)
Cpsi = cos(psi)
C11 = 0.
C12 = ((Iyy - Izz) * (thetadot * Cphi * Sphi +
psidot * Sphi ** 2 *Ctheta) +
(Izz - Iyy) * psidot * Cphi ** 2 *Ctheta -
Ixx * psidot * Ctheta)
C13 = (Izz - Iyy) * psidot * Cphi * Sphi * Ctheta ** 2
C21 = ((Izz - Iyy) * (thetadot * Cphi * Sphi +
psidot * Sphi * Ctheta) +
(Iyy - Izz) * psidot * Cphi ** 2 * Ctheta -
Ixx * psidot * Ctheta)
C22 = (Izz - Iyy) * phidot * Cphi * Sphi
C23 = (-Ixx * psidot * Stheta * Ctheta +
Iyy * psidot * Sphi ** 2 * Stheta * Ctheta +
Izz * psidot * Cphi ** 2 * Stheta * Ctheta)
C31 = ((Iyy - Izz) * psidot * Ctheta ** 2 * Sphi * Cphi -
Ixx * thetadot * Ctheta)
C32 = ((Izz - Iyy) * (thetadot * Cphi * Sphi * Stheta +
phidot * Sphi ** 2 * Ctheta) +
(Iyy - Izz) * phidot * Cphi ** 2 * Ctheta +
Ixx * psidot * Stheta * Ctheta -
Iyy * psidot * Sphi ** 2 * Stheta * Ctheta -
Izz * psi * Cphi ** 2 * Stheta * Ctheta)
C33 = ((Iyy - Izz) * phidot * Cphi * Sphi * Ctheta ** 2 -
Iyy * thetadot * Sphi ** 2 * Ctheta * Stheta -
Izz * thetadot * Cphi ** 2 * Ctheta * Stheta +
Ixx * thetadot * Ctheta * Stheta)
C = np.array([[C11,C12,C13],
[C21,C22,C23],
[C31,C32,C33]]);
# eq. (8) Modified from the work of lukkonen, here rotor 2 and 4 spin in
# the opposite directions
tau_phi = l * k * (-omega[1] ** 2 + omega[3] ** 2)
tau_theta = l * k * (-omega[0] ** 2 + omega[2] ** 2)
tau_psi = b * (omega[0] ** 2 - omega[1] ** 2 +
omega[2] ** 2 - omega[3] ** 2)
tau_b = np.array([[tau_phi],[tau_theta],[tau_psi]])
# eq. (16)
J11 = Ixx
J12 = 0.
J13 = -Ixx*Stheta
J21 = 0.
J22 = Iyy * Cphi ** 2 + Izz * Sphi ** 2
J23 = (Iyy - Izz) * Cphi * Sphi * Ctheta
J31 = -Ixx * Stheta
J32 = (Iyy - Izz) * Cphi * Sphi * Ctheta
J33 = (Ixx * Stheta ** 2 + Iyy * Sphi ** 2 * Ctheta ** 2 +
Izz * Cphi ** 2 * Ctheta ** 2)
J = np.array([[J11,J12,J13],
[J21,J22,J23],
[J31,J32,J33]])
invJ = spl.inv(J)
# Sets up continuous system
I3 = np.diag(np.ones([1,3])[0])
Ac = np.zeros([12,12])
Ac[0:3,3:6] = I3
Ac[3:6,3:6] = -(1/m)*np.diag(A)
Ac[6:9,9:12] = I3
Ac[9:12,9:12] = -np.dot(invJ,C)
Rz = (1/m) * np.array([[cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi)],
[sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi)],
[cos(theta) * cos(phi)]])
Bc = np.zeros([12,4])
Bc[3:6,0:1] = Rz
Bc[9:12,1:4] = invJ
Cc = np.zeros([6,12])
Cc[0,0] = 1. # x
Cc[1,1] = 1. # y
Cc[2,2] = 1. # z
Cc[3,6] = 1. # phi
Cc[4,7] = 1. # theta
Cc[5,8] = 1. # psi
Dc = np.array([])
discreteSystem = cont2discrete((Ac,Bc,Cc,Dc), Ts, method='zoh', alpha=None)
Ad = discreteSystem[0]
Bd = discreteSystem[1]
Cd = discreteSystem[2]
Dd = discreteSystem[3]
G = np.zeros([12,1])
G[5,0]=-g
# Sets up control signal
u = np.concatenate([[[T]],tau_b])
xout = np.dot(Ad,x) + np.dot(Bd,u) + G*Ts
yout = np.dot(Cd,x)
self.x = xout
self.measured_states_pub.publish(xout)
def sample_system(sysc, Ts, method='matched'):
"""Convert a continuous time system to discrete time
Creates a discrete time system from a continuous time system by
sampling. Multiple methods of conversion are supported.
Parameters
----------
sysc : linsys
Continuous time system to be converted
Ts : real
Sampling period
method : string
Method to use for conversion: 'matched' (default), 'tustin', 'zoh'
Returns
-------
sysd : linsys
Discrete time system, with sampling rate Ts
Notes
-----
1. The conversion methods 'tustin' and 'zoh' require the
cont2discrete() function, including in SciPy 0.10.0 and above.
2. Additional methods 'foh' and 'impulse' are planned for future
implementation.
Examples
--------
>>> sysc = TransferFunction([1], [1, 2, 1])
>>> sysd = sample_system(sysc, 1, method='matched')
"""
# Make sure we have a continuous time system
if not isctime(sysc):
raise ValueError("First argument must be continuous time system")
# TODO: impelement MIMO version
if (sysc.inputs != 1 or sysc.outputs != 1):
raise NotImplementedError("MIMO implementation not available")
# If we are passed a state space system, convert to transfer function first
if isinstance(sysc, StateSpace):
warn("sample_system: converting to transfer function")
sysc = _convertToTransferFunction(sysc)
# Decide what to do based on the methods available
if method == 'matched':
sysd = _c2dmatched(sysc, Ts)
elif method == 'tustin':
try:
from scipy.signal import cont2discrete
sys = [sysc.num[0][0], sysc.den[0][0]]
scipySysD = cont2discrete(sys, Ts, method='bilinear')
sysd = TransferFunction(scipySysD[0][0], scipySysD[1], Ts)
except ImportError:
raise TypeError("cont2discrete not found in scipy.signal; upgrade to v0.10.0+")
elif method == 'zoh':
try:
from scipy.signal import cont2discrete
sys = [sysc.num[0][0], sysc.den[0][0]]
scipySysD = cont2discrete(sys, Ts, method='zoh')
sysd = TransferFunction(scipySysD[0][0],scipySysD[1], Ts)
except ImportError:
raise TypeError("cont2discrete not found in scipy.signal; upgrade to v0.10.0+")
elif method == 'foh' or method == 'impulse':
raise ValueError("Method not developed yet")
else:
raise ValueError("Invalid discretization method: %s" % method)
# TODO: Convert back into the input form
# Set sampling time
return sysd
def evaluate_system(self, x, u):
phi, theta, psi, phidot, thetadot, psidot = np.reshape(x[6:12,0:1],[1,6])[0]
Ts = self.Ts
# Local parameters
g = self.g
m = self.m
k = self.k
A = self.A
I = self.I
l = self.l
b = self.b
u = u[:,0]
T = k * sum(u ** 2)
# Define the C-matrix, see eq. (19)
Ixx, Iyy, Izz = I
Sphi = sin(phi)
Stheta = sin(theta)
Spsi = sin(psi);
Cphi = cos(phi)
Ctheta = cos(theta)
Cpsi = cos(psi)
C11 = 0.
C12 = ((Iyy - Izz) * (thetadot * Cphi * Sphi +
psidot * Sphi ** 2 *Ctheta) +
(Izz - Iyy) * psidot * Cphi ** 2 *Ctheta -
Ixx * psidot * Ctheta)
C13 = (Izz - Iyy) * psidot * Cphi * Sphi * Ctheta ** 2
C21 = ((Izz - Iyy) * (thetadot * Cphi * Sphi +
psidot * Sphi * Ctheta) +
(Iyy - Izz) * psidot * Cphi ** 2 * Ctheta -
Ixx * psidot * Ctheta)
C22 = (Izz - Iyy) * phidot * Cphi * Sphi
C23 = (-Ixx * psidot * Stheta * Ctheta +
Iyy * psidot * Sphi ** 2 * Stheta * Ctheta +
Izz * psidot * Cphi ** 2 * Stheta * Ctheta)
C31 = ((Iyy - Izz) * psidot * Ctheta ** 2 * Sphi * Cphi -
Ixx * thetadot * Ctheta)
C32 = ((Izz - Iyy) * (thetadot * Cphi * Sphi * Stheta +
phidot * Sphi ** 2 * Ctheta) +
(Iyy - Izz) * phidot * Cphi ** 2 * Ctheta +
Ixx * psidot * Stheta * Ctheta -
Iyy * psidot * Sphi ** 2 * Stheta * Ctheta -
Izz * psi * Cphi ** 2 * Stheta * Ctheta)
C33 = ((Iyy - Izz) * phidot * Cphi * Sphi * Ctheta ** 2 -
Iyy * thetadot * Sphi ** 2 * Ctheta * Stheta -
Izz * thetadot * Cphi ** 2 * Ctheta * Stheta +
Ixx * thetadot * Ctheta * Stheta)
C = np.array([[C11,C12,C13],
[C21,C22,C23],
[C31,C32,C33]]);
# eq. (8) Modified from the work of lukkonen, here rotor 2 and 4 spin in
# the opposite directions
tau_phi = l * k * (-u[1] ** 2 + u[3] ** 2)
tau_theta = l * k * (-u[0] ** 2 + u[2] ** 2)
tau_psi = b * (u[0] ** 2 - u[1] ** 2 +
u[2] ** 2 - u[3] ** 2)
tau_b = np.array([[tau_phi],[tau_theta],[tau_psi]])
# eq. (16)
J11 = Ixx
J12 = 0.
J13 = -Ixx*Stheta
J21 = 0.
J22 = Iyy * Cphi ** 2 + Izz * Sphi ** 2
J23 = (Iyy - Izz) * Cphi * Sphi * Ctheta
J31 = -Ixx * Stheta
J32 = (Iyy - Izz) * Cphi * Sphi * Ctheta
J33 = (Ixx * Stheta ** 2 + Iyy * Sphi ** 2 * Ctheta ** 2 +
Izz * Cphi ** 2 * Ctheta ** 2)
J = np.array([[J11,J12,J13],
[J21,J22,J23],
[J31,J32,J33]])
invJ = sp.linalg.inv(J)
# Sets up continuous system
I3 = np.diag(np.ones([1,3])[0])
Ac = np.zeros([12,12])
Ac[0:3,3:6] = I3
Ac[3:6,3:6] = -(1/m)*np.diag(A)
Ac[6:9,9:12] = I3
Ac[9:12,9:12] = -np.dot(invJ,C)
Rz = (1/m) * np.array([[cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi)],
[sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi)],
[cos(theta) * cos(phi)]])
Bc = np.zeros([12,4])
Bc[3:6,0:1] = Rz
Bc[9:12,1:4] = invJ
Cc = np.zeros([7,12])
Cc[0:7,2:9] = np.diag(np.ones([1,7])[0]) # z,rdot,eta
Dc = np.array([])
discreteSystem = cont2discrete((Ac,Bc,Cc,Dc), Ts, method='zoh', alpha=None)
Ad = discreteSystem[0]
Bd = discreteSystem[1]
Cd = discreteSystem[2]
G = np.zeros([12,1])
G[5,0]=-g
# Sets up control signal
u = np.concatenate([[[T]],tau_b])
xout = np.dot(Ad,x) + np.dot(Bd,u) + G*Ts
yout = np.dot(Cd,x)
return xout, yout
def RunFilter(self,x_vec,t_vec):
den = signal.convolve(np.array([self.Ts1,1]),np.array([self.Ts2,1]));
num = self.dc_gain;
filter_obj = signal.cont2discrete((num,den),self.Ts,method='zoh');
tout, x_filt= signal.dlsim(filter_obj,x_vec,t=t_vec);
return x_filt;
def test_detector():
fitSamples = 100
# Prepare detector
zero_1 = -5.56351644e+07
pole_1 = -1.38796386e+04
pole_real = -2.02559385e+07
pole_imag = 9885315.37450211
gain = 2./1e-8
zeros = [zero_1,0. ]
poles = [ pole_real+pole_imag*1j, pole_real-pole_imag*1j, pole_1]
system = signal.lti(zeros, poles, gain )
system.to_tf()
num = system.num
den = system.den
print "original num: " + str(num)
print "original den" + str(den)
system.to_zpk()
print "back to the zpk"
print system.zeros
print system.poles
print system.gain
# num = np.array((3478247474.8078203, 1.9351287044375424e+17, 6066014749714584.0))
# den = [1, 40525756.715025946, 508584795912802.44, 7.0511687850000589e+18]
# system = signal.lti(num, den)
#
# zeros = system.zeros
# poles = system.poles
# gain = system.gain
new_num, new_den, dt = signal.cont2discrete((num, den), 1E-8, method="bilinear")
# new_num /= 1.05836038e-08
print "new discrete tf representation"
print new_num
print new_den
print dt
new_z, new_p, new_k, dt = signal.cont2discrete((zeros, poles, gain), 1E-8, method="bilinear" )
print "new discrete zpk representation"
print new_z
print new_p
print new_k
print dt
print "...and back to tf"
dis_num, dis_den = signal.zpk2tf(new_z, new_p, new_k)
print dis_num
print dis_den
tempGuess = 77.89
gradGuess = 0.0483
pcRadGuess = 2.591182
pcLenGuess = 1.613357
#Create a detector model
detName = "conf/P42574A_grad%0.2f_pcrad%0.2f_pclen%0.2f.conf" % (0.05,2.5, 1.65)
det = Detector(detName, temperature=tempGuess, timeStep=10., numSteps=fitSamples, )
det.LoadFields("P42574A_fields_v3.npz")
det.SetFields(pcRadGuess, pcLenGuess, gradGuess)
print "time steps out number is %d" % det.siggenInst.GetSafeConfiguration()['ntsteps_out']
print "time steps calc number is %d" % det.siggenInst.GetSafeConfiguration()['time_steps_calc']
print "step size out is %f" % det.siggenInst.GetSafeConfiguration()['step_time_out']
print "step size calc is %f" % det.siggenInst.GetSafeConfiguration()['step_time_calc']
sig_array = det.GetRawSiggenWaveform( 0.174070, 0.528466, 10.755795)
t, sig_array2, x = signal.lsim(system, sig_array, det.time_steps, interp=False)
sig_array1 = signal.lfilter(new_num[0], new_den, sig_array)
sig_array4 = signal.lfilter(dis_num, dis_den, sig_array)
# dissystem = system.to_discrete(1E-8)
#
## new_k = dissystem.zeros
## new_p = dissystem.poles
## new_k = dissystem.gain
## dis_num, dis_den = signal.zpk2tf(new_z, new_p, new_k)
## sig_array5 = signal.lfilter(dis_num, dis_den, sig_array)
#
#
plt.figure()
plt.plot(sig_array1, "b:")
plt.plot(sig_array2, color="black")
plt.plot(sig_array4, "r:")
# plt.plot(sig_array5, "g:")
plt.figure()
plt.plot(sig_array2-sig_array1, color="b")
plt.plot(sig_array2-sig_array4, color="r")
# plt.plot(sig_array2-sig_array5, color="g")
#plt.ylim(-0.05, 1.05)
plt.xlabel("Time [ns]")
plt.ylabel("Charge [arb.]")
plt.show()
def test_lti():
import scipy.signal as sps
dt = 1e-3
tend = 10.
t = dt * np.arange(tend / dt)
nt = len(t)
tau = 1e-2
tf = sps.cont2discrete(([1], [tau, 1]), dt, method='euler')[:2]
# tf = sps.cont2discrete(([1], [tau**2, 2*tau, 1]), dt, method='zoh')[:2]
# tf = sps.butter(6, dt / (np.pi * tau))
# tf = sps.cheby1(5, 40, dt / (np.pi * tau))
print tf
# tf2 = sps.cont2discrete(([1], [tau, 1]), dt, method='zoh')[:2]
tf2 = sps.cont2discrete(([1], [tau, 1]), dt, method='gbt', alpha=0.5)[:2]
### plot filtered signal and spectrum
x = np.random.normal(size=(nt,1000))
y = neurotools.filters.lti(x, tf)
y2 = neurotools.filters.lti(x, tf2)
plt.figure(1)
plt.clf()
plt.subplot(211)
plt.plot(t, y[:,0])
plt.plot(t, y2[:,0])
f, mag, ang = get_spectrum(t, x, y)
f2, mag2, ang2 = get_spectrum(t, x, y2)
plt.subplot(223)
plt.loglog(f, mag)
plt.loglog(f2, mag2)
plt.subplot(224)
delay = ang / (2 * np.pi * f)
delay2 = ang2 / (2 * np.pi * f2)
plt.semilogx(f[1:], delay[1:])
plt.semilogx(f2[1:], delay2[1:])
### plot impulse response
tend = 10 * tau
t = dt * np.arange(tend / dt)
nt = len(t)
x = np.zeros(nt)
x[0] = 1. / dt
y = neurotools.filters.lti(x, tf)
plt.figure(2)
plt.clf()
plt.plot(t, y)
print("Impulse response sum:", y.sum() * dt)
### plot sine input
x = np.sin(3 * (2 * np.pi) * t)
y = neurotools.filters.lti(x, tf)
plt.figure(3)
plt.clf()
plt.plot(t, x)
plt.plot(t, y)
plt.show()
def discretize(self, samplerate):
systf = (self.num, self.den)
(dnum, dden, _) = signal.cont2discrete(systf, 1 / samplerate)
return (np.squeeze(dnum), np.squeeze(dden))
def quadcopter_dynamics(x, u, param):
# The model of the quadcopter which is used in non-linear state estimation
# on the host and for simulation purposes. The state is updated internally
# and updated on every time step with reference to the input control signal
# omega. The parameters defining the process and states are loaded from
# a parameter dictionary on the same for as that used in the configuration
# file (see /config/*).
#
# ARGS:
# u - np.array of shape (4,1) - control signals (omega)
# RETURN:
# xout - np.array of shape (12,1) - state vector with on the form
# [r,dr,eta,deta]^T.
# yout - np.array of shape (12,1) - Measuereent vector returning
# certain states as specified in C.
# Extract angles and parameters
phi, theta, psi, phidot, thetadot, psidot = np.reshape(x[6:12,0:1],[1,6])[0]
Ts = param['global']['inner_loop_h']
g = param['quadcopter_model']['g']
m = param['quadcopter_model']['m']
k = param['quadcopter_model']['k']
A = param['quadcopter_model']['A']
I = param['quadcopter_model']['I']
l = param['quadcopter_model']['l']
b = param['quadcopter_model']['b']
u = u[:,0]
T = k * sum(u ** 2)
# Define the C-matrix, see eq. (19)
Ixx, Iyy, Izz = I
Sphi = sin(phi)
Stheta = sin(theta)
Spsi = sin(psi);
Cphi = cos(phi)
Ctheta = cos(theta)
Cpsi = cos(psi)
C11 = 0.
C12 = ((Iyy - Izz) * (thetadot * Cphi * Sphi +
psidot * Sphi ** 2 *Ctheta) +
(Izz - Iyy) * psidot * Cphi ** 2 *Ctheta -
Ixx * psidot * Ctheta)
C13 = (Izz - Iyy) * psidot * Cphi * Sphi * Ctheta ** 2
C21 = ((Izz - Iyy) * (thetadot * Cphi * Sphi +
psidot * Sphi * Ctheta) +
(Iyy - Izz) * psidot * Cphi ** 2 * Ctheta -
Ixx * psidot * Ctheta)
C22 = (Izz - Iyy) * phidot * Cphi * Sphi
C23 = (-Ixx * psidot * Stheta * Ctheta +
Iyy * psidot * Sphi ** 2 * Stheta * Ctheta +
Izz * psidot * Cphi ** 2 * Stheta * Ctheta)
C31 = ((Iyy - Izz) * psidot * Ctheta ** 2 * Sphi * Cphi -
Ixx * thetadot * Ctheta)
C32 = ((Izz - Iyy) * (thetadot * Cphi * Sphi * Stheta +
phidot * Sphi ** 2 * Ctheta) +
(Iyy - Izz) * phidot * Cphi ** 2 * Ctheta +
Ixx * psidot * Stheta * Ctheta -
Iyy * psidot * Sphi ** 2 * Stheta * Ctheta -
Izz * psi * Cphi ** 2 * Stheta * Ctheta)
C33 = ((Iyy - Izz) * phidot * Cphi * Sphi * Ctheta ** 2 -
Iyy * thetadot * Sphi ** 2 * Ctheta * Stheta -
Izz * thetadot * Cphi ** 2 * Ctheta * Stheta +
Ixx * thetadot * Ctheta * Stheta)
C = np.array([[C11,C12,C13],
[C21,C22,C23],
[C31,C32,C33]]);
# eq. (8) Modified from the work of lukkonen, here rotor 2 and 4 spin in
# the opposite directions
tau_phi = l * k * (-u[1] ** 2 + u[3] ** 2)
tau_theta = l * k * (-u[0] ** 2 + u[2] ** 2)
tau_psi = b * (u[0] ** 2 - u[1] ** 2 +
u[2] ** 2 - u[3] ** 2)
tau_b = np.array([[tau_phi],[tau_theta],[tau_psi]])
# eq. (16)
J11 = Ixx
J12 = 0.
J13 = -Ixx*Stheta
J21 = 0.
J22 = Iyy * Cphi ** 2 + Izz * Sphi ** 2
J23 = (Iyy - Izz) * Cphi * Sphi * Ctheta
J31 = -Ixx * Stheta
J32 = (Iyy - Izz) * Cphi * Sphi * Ctheta
J33 = (Ixx * Stheta ** 2 + Iyy * Sphi ** 2 * Ctheta ** 2 +
Izz * Cphi ** 2 * Ctheta ** 2)
J = np.array([[J11,J12,J13],
[J21,J22,J23],
[J31,J32,J33]])
invJ = inv(J)
# Sets up continuous system
I3 = np.diag(np.ones([1,3])[0])
Ac = np.zeros([12,12])
Ac[0:3,3:6] = I3
Ac[3:6,3:6] = -(1/m)*np.diag(A)
Ac[6:9,9:12] = I3
Ac[9:12,9:12] = -np.dot(invJ,C)
Rz = (1/m) * np.array([[cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi)],
[sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi)],
[cos(theta) * cos(phi)]])
Bc = np.zeros([12,4])
Bc[3:6,0:1] = Rz
Bc[9:12,1:4] = invJ
Cc = np.zeros([7,12])
Cc[0:7,2:9] = np.diag(np.ones([1,7])[0]) # z,rdot,eta
Dc = np.array([])
discreteSystem = cont2discrete((Ac,Bc,Cc,Dc), Ts, method='zoh', alpha=None)
Ad = discreteSystem[0]
Bd = discreteSystem[1]
Cd = discreteSystem[2]
G = np.zeros([12,1])
G[5,0]=-g
# Sets up control signal
u = np.concatenate([[[T]],tau_b])
xout = np.dot(Ad,x) + np.dot(Bd,u) + G*Ts
yout = np.dot(Cd,x)
return xout, yout
def mapCtoD(sys_c, t=(0, 1), f0=0.0):
"""Map a MIMO continuous-time to an equiv. SIMO discrete-time system.
The criterion for equivalence is that the sampled pulse response
of the CT system must be identical to the impulse response of the DT system.
i.e. If ``yc`` is the output of the CT system with an input ``vc`` taken
from a set of DACs fed with a single DT input ``v``, then ``y``, the output
of the equivalent DT system with input ``v`` satisfies:
``y(n) = yc(n-)`` for integer ``n``. The DACs are characterized by
rectangular impulse responses with edge times specified in the t list.
**Input:**
sys_c : object
the LTI description of the CT system, which can be:
* the ABCD matrix,
* a list-like containing the A, B, C, D matrices,
* a list of zpk tuples (internally converted to SS representation),
* a list of LTI objects.
t : array_like
The edge times of the DAC pulse used to make CT waveforms
from DT inputs. Each row corresponds to one of the system
inputs; [-1 -1] denotes a CT input. The default is [0 1],
for all inputs except the first.
f0 : float
The (normalized) frequency at which the Gp filters' gains are
to be set to unity. Default 0 (DC).
**Output:**
sys : tuple
the LTI description for the DT equivalent, in A, B, C, D
representation.
Gp : list of lists
the mixed CT/DT prefilters which form the samples
fed to each state for the CT inputs.
**Example:**
Map the standard second order CT modulator shown below to its CT
equivalent and verify that its NTF is :math:`(1-z^{-1})^2`.
.. image:: ../doc/_static/mapCtoD.png
:align: center
:alt: mapCtoD block diagram
It can be done as follows::
from __future__ import print_function
import numpy as np
from scipy.signal import lti
from deltasigma import *
LFc = lti([[0, 0], [1, 0]], [[1, -1], [0, -1.5]], [[0, 1]], [[0, 0]])
tdac = [0, 1]
LF, Gp = mapCtoD(LFc, tdac)
LF = lti(*LF)
ABCD = np.vstack((
np.hstack((LF.A, LF.B)),
np.hstack((LF.C, LF.D))
))
NTF, STF = calculateTF(ABCD)
print("NTF:") # after rounding to a 1e-6 resolution
print("Zeros:", np.real_if_close(np.round(NTF.zeros, 6)))
print("Poles:", np.real_if_close(np.round(NTF.poles, 6)))
Prints::
Zeros: [ 1. 1.]
Poles: [ 0. 0.]
Equivalent to::
(z -1)^2
NTF = ----------
z^2
.. seealso:: R. Schreier and B. Zhang, "Delta-sigma modulators employing \
continuous-time circuitry," IEEE Transactions on Circuits and Systems I, \
vol. 43, no. 4, pp. 324-332, April 1996.
"""
# You need to have A, B, C, D specification of the system
Ac, Bc, Cc, Dc = _getABCD(sys_c)
ni = Bc.shape[1]
# Sanitize t
if hasattr(t, "tolist"):
t = t.tolist()
if (type(t) == tuple or type(t) == list) and np.isscalar(t[0]):
t = [t] # we got a simple list, like the default value
if not (type(t) == tuple or type(t) == list) and not (type(t[0]) == tuple or type(t[0]) == list):
raise ValueError("The t argument has an unrecognized shape")
# back to business
t = np.array(t)
if t.shape == (1, 2) and ni > 1:
t = np.vstack((np.array([[-1, -1]]), np.dot(np.ones((ni - 1, 1)), t)))
if t.shape != (ni, 2):
raise ValueError("The t argument has the wrong dimensions.")
di = np.ones(ni).astype(bool)
for i in range(ni):
if t[i, 0] == -1 and t[i, 1] == -1:
di[i] = False
# c2d assumes t1=0, t2=1.
# Also c2d often complains about poor scaling and can even produce
# incorrect results.
A, B, C, D, _ = cont2discrete((Ac, Bc, Cc, Dc), 1, method="zoh")
Bc1 = Bc[:, ~di]
# Examine the discrete-time inputs to see how big the
# augmented matrices need to be.
B1 = B[:, ~di]
D1 = D[:, ~di]
n = A.shape[0]
t2 = np.ceil(t[di, 1]).astype(np.int_)
esn = (t2 == t[di, 1]) and (D[0, di] != 0).T # extra states needed?
npp = n + np.max(t2 - 1 + 1 * esn)
# Augment A to npp x npp, B to np x 1, C to 1 x np.
Ap = padb(padr(A, npp), npp)
for i in range(n + 1, npp):
Ap[i, i - 1] = 1
Bp = np.zeros((npp, 1))
if npp > n:
Bp[n, 0] = 1
Cp = padr(C, npp)
Dp = np.zeros((1, 1))
# Add in the contributions from each DAC
for i in np.flatnonzero(di):
t1 = t[i, 0]
t2 = t[i, 1]
B2 = B[:, i]
D2 = D[:, i]
if t1 == 0 and t2 == 1 and D2 == 0: # No fancy stuff necessary
Bp = Bp + padb(B2, npp)
else:
n1 = np.floor(t1)
n2 = np.ceil(t2) - n1 - 1
t1 = t1 - n1
t2 = t2 - n2 - n1
if t2 == 1 and D2 != 0:
n2 = n2 + 1
extraStateNeeded = 1
else:
extraStateNeeded = 0
nt = n + n1 + n2
if n2 > 0:
if t2 == 1:
Ap[:n, nt - n2 : nt] = Ap[:n, nt - n2 : nt] + np.tile(B2, (1, n2))
else:
Ap[:n, nt - n2 : nt - 1] = Ap[:n, nt - n2 : nt - 1] + np.tile(B2, (1, n2 - 1))
Ap[:n, (nt - 1)] = Ap[:n, (nt - 1)] + _B2formula(Ac, 0, t2, B2)
if n2 > 0: # pulse extends to the next period
Btmp = _B2formula(Ac, t1, 1, B2)
else: # pulse ends in this period
Btmp = _B2formula(Ac, t1, t2, B2)
if n1 > 0:
Ap[:n, n + n1 - 1] = Ap[:n, n + n1 - 1] + Btmp
else:
Bp = Bp + padb(Btmp, npp)
if n2 > 0:
Cp = Cp + padr(np.hstack((np.zeros((D2.shape[0], n + n1)), D2 * np.ones((1, n2)))), npp)
sys = (Ap, Bp, Cp, Dp)
if np.any(~di):
# Compute the prefilters and add in the CT feed-ins.
# Gp = inv(sI - Ac)*(zI - A)/z*Bc1
n, m = Bc1.shape
Gp = np.empty_like(np.zeros((n, m)), dtype=object)
# !!Make this like stf: an array of zpk objects
ztf = np.empty_like(Bc1, dtype=object)
# Compute the z-domain portions of the filters
ABc1 = np.dot(A, Bc1)
for h in range(m):
for i in range(n):
if Bc1[i, h] == 0:
ztf[i, h] = (np.array([]), np.array([0.0]), -ABc1[i, h]) # dt=1
else:
ztf[i, h] = (np.atleast_1d(ABc1[i, h] / Bc1[i, h]), np.array([0.0]), Bc1[i, h]) # dt = 1
# Compute the s-domain portions of each of the filters
stf = np.empty_like(np.zeros((n, n)), dtype=object) # stf[out, in] = zpk
for oi in range(n):
for ii in range(n):
# Doesn't do pole-zero cancellation
stf[oi, ii] = ss2zpk(Ac, np.eye(n), np.eye(n)[oi, :], np.zeros((1, n)), input=ii)
# scipy as of v 0.13 has no support for LTI MIMO systems
# only 'MISO', therefore you can't write:
# stf = ss2zpk(Ac, eye(n), eye(n), np.zeros(n, n)))
for h in range(m):
for i in range(n):
# k = 1 unneded, see below
for j in range(n):
# check the k values for a non-zero term
if stf[i, j][2] != 0 and ztf[j, h][2] != 0:
if Gp[i, h] is None:
Gp[i, h] = {}
Gp[i, h].update({"Hs": [list(stf[i, j])]})
Gp[i, h].update({"Hz": [list(ztf[j, h])]})
else:
Gp[i, h].update({"Hs": Gp[i, h]["Hs"] + [list(stf[i, j])]})
Gp[i, h].update({"Hz": Gp[i, h]["Hz"] + [list(ztf[j, h])]})
# the MATLAB-like cell code for the above statements would have
# been:
# Gp[i, h](k).Hs = stf[i, j]
# Gp[i, h](k).Hz = ztf[j, h]
# k = k + 1
if f0 != 0: # Need to correct the gain terms calculated by c2d
# B1 = gains of Gp @f0;
for h in range(m):
for i in range(n):
B1ih = np.real_if_close(evalMixedTF(Gp[i, h], f0))
# abs() used because ss() whines if B has complex entries...
# This is clearly incorrect.
# I've fudged the complex stuff by including a sign....
B1[i, h] = np.abs(B1ih) * np.sign(np.real(B1ih))
if np.abs(B1[i, h]) < 1e-09:
B1[i, h] = 1e-09 # This prevents NaN in "line 174" below
# Adjust the gains of the pre-filters
for h in range(m):
for i in range(n):
for j in range(max(len(Gp[i, h]["Hs"]), len(Gp[i, h]["Hz"]))):
# The next is "line 174"
Gp[i, h]["Hs"][j][2] = Gp[i, h]["Hs"][j][2] / B1[i, h]
sys = (
sys[0], # Ap
np.hstack((padb(B1, npp), sys[1])), # new B
sys[2], # Cp
np.hstack((D1, sys[3])),
) # new D
return sys, Gp
def cont2discrete(sys, dt, method='bilinear'):
discrete_sys = signal.cont2discrete(sys, dt, method=method)[:-1]
if len(discrete_sys) == 2:
discrete_sys = tuple(np.squeeze(b_or_a) for b_or_a in discrete_sys)
return discrete_sys
def compute_gains(Q, R, W, V, dt):
"""Given LQR Q and R matrices, and Kalman W and V matrices, and sample
time, compute optimal feedback gain and optimal filter gains."""
data = np.empty((N,), dtype=controller_t)
# Loop over all speeds for which we have system dynamics
for i in range(N):
data['theta_R_dot'][i] = theta_R_dot[i]
data['dt'][i] = dt
# Convert the bike dynamics to discrete time using a zero order hold
data['A'][i], data['B'][i], _, _, _ = cont2discrete(
(A_w[i], B_w[i, :], eye(4), zeros((4, 1))), dt)
data['plant_evals_d'][i] = la.eigvals(data['A'][i])
data['plant_evals_c'][i] = np.log(data['plant_evals_d'][i]) / dt
# Bicycle measurement matrices
# - steer angle
# - roll rate
data['C_m'][i] = C_w[i, :2, :]
# - yaw rate
data['C_z'][i] = C_w[i, 2, :]
A = data['A'][i]
B = data['B'][i, :, 2].reshape((4, 1))
C_m = data['C_m'][i]
C_z = data['C_z'][i]
# Controllability from steer torque
data['ctrb_plant'][i] = ctrb(A, B)
u, s, v = la.svd(data['ctrb_plant'][i])
assert(np.all(s > 1e-13))
# Solve discrete algebraic Ricatti equation associated with LQI problem
P_c = dare(A, B, R, Q)
# Optimal feedback gain using solution of Ricatti equation
K_c = -la.solve(R + dot(B.T, dot(P_c, B)),
dot(B.T, dot(P_c, A)))
data['K_c'][i] = K_c
data['A_c'][i] = A + dot(B, K_c)
data['B_c'][i] = B
data['controller_evals'][i] = la.eigvals(data['A_c'][i])
data['controller_evals_c'][i] = np.log(data['controller_evals'][i]) / dt
assert(np.all(abs(data['controller_evals'][i]) < 1.0))
# Observability from steer angle and roll rate measurement
# Note that (A, C_m * A) must be observable in the "current estimator"
# formulation
data['obsv_plant'][i] = obsv(A, dot(C_m, A))
u, s, v = la.svd(data['obsv_plant'][i])
assert(np.all(s > 1e-13))
# Solve Riccati equation
P_e = dare(A.T, C_m.T, V, W)
# Compute Kalman gain
K_e = dot(P_e, dot(C_m.T, la.inv(dot(C_m, dot(P_e, C_m.T)) + V)))
data['K_e'][i] = K_e
data['A_e'][i] = dot(eye(4) - dot(K_e, C_m), A)
data['B_e'][i] = np.hstack((dot(eye(4) - dot(K_e, C_m), B), K_e))
data['estimator_evals'][i] = la.eigvals(data['A_e'][i])
data['estimator_evals_c'][i] = np.log(data['estimator_evals'][i]) / dt
# Verify that Kalman estimator eigenvalues are stable
assert(np.all(abs(data['estimator_evals'][i]) < 1.0))
# Closed loop state space equations
A_cl = np.zeros((8, 8))
A_cl[:4, :4] = A
A_cl[:4, 4:] = dot(B, K_c)
A_cl[4:, :4] = dot(K_e, dot(C_m, A))
A_cl[4:, 4:] = A - A_cl[4:, :4] + A_cl[:4, 4:]
data['A_cl'][i] = A_cl
data['closed_loop_evals'][i] = la.eigvals(A_cl)
assert(np.all(abs(data['closed_loop_evals'][i]) < 1.0))
B_cl = np.zeros((8, 1))
B_cl[:4, 0] = B.reshape((4,))
B_cl[4:, 0] = dot(eye(4) - dot(K_e, C_m), B).reshape((4,))
data['B_cl'][i] = B_cl
C_cl = np.hstack((C_z, np.zeros((1, 4))))
data['C_cl'][i] = C_cl
# Transfer functions from r to yaw rate
num, den = ss2tf(A_cl, B_cl, C_cl, 0)
data['w_r_to_psi_dot'][i], y = freqz(num[0], den)
data['w_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
# Open loop transfer function from e to yaw rate (PI loop not closed,
# but LQR/LQG loop closed.
inner_cl = ss(A_cl, B_cl, C_cl, 0)
pi_block = ss([[1]], [[1]], [[data['Ki_fit'][i]*dt]], [[data['Kp_fit'][i]]])
e_to_psi_dot = series(pi_block, inner_cl)
num, den = ss2tf(e_to_psi_dot.A, e_to_psi_dot.B, e_to_psi_dot.C, e_to_psi_dot.D)
data['w_e_to_psi_dot'][i], y = freqz(num[0], den)
data['w_e_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_e_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_e_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
return data
print A.T.dot(P) + P.dot(A) R = np.eye(2) S = 1.0 K = E.T.dot(P).dot(R).dot(P.T).dot(E) vk,V = np.linalg.eig(K) print vk b = 2*np.sqrt(S*np.max(vk)/np.min(vp)) v,V = np.linalg.eig(A) print v Ad,Bd,Cd,Dd,dt = cont2discrete((A,B,np.eye(2),np.eye(2)), dt, method='zoh') x0 = np.array([[1.5,0]]).T x = x0 t = 0 X = [x0] T = [t] U = [np.array([[0,0]]).T] _B = np.linalg.inv(Bd) while t<10.0: if (t<3): _u = -x.copy() if t%2.<2.0*dt and ( t>5): _u = x.copy()
def quadcopter_dynamics(x, u, param):
"""The Tait-Bryan model of the quadcopter which is used in non-linear
state estimation on the host and for simulation purposes. The state is
updated internally and updated on every time step with reference to the
input control signal omega. The parameters defining the process and states
are loaded from a parameter dictionary on the same for as that used in the
configuration file (see /config/).
:param x: State vector x = [p, dp, eta, deta]^T at a time k.
:param u: Control signals on the form [T, tau_x, tau_y, tau_z]^T at a time k.
:param param: Parameter dictionary with quadcopter coefficients.
:type x: np.array of shape (12,1)
:type u: np.array of shape (4,1)
:type param: Dictionary
:returns: xout, yout
:rtype: np.array (12,1), np.array (M,1) as specified in C
"""
# Extract angles and parameters
phi, theta, psi, phidot, thetadot, psidot = np.reshape(x[6:12,0:1],[1,6])[0]
Ts = param['global']['inner_loop_h']
g = param['quadcopter_model']['g']
m = param['quadcopter_model']['m']
k = param['quadcopter_model']['k']
A = param['quadcopter_model']['A']
I = param['quadcopter_model']['I']
l = param['quadcopter_model']['l']
b = param['quadcopter_model']['b']
u = u[:,0]
T = k * sum(u ** 2)
Ixx, Iyy, Izz = I
Sphi = sin(phi)
Stheta = sin(theta)
Spsi = sin(psi);
Cphi = cos(phi)
Ctheta = cos(theta)
Cpsi = cos(psi)
C11 = 0.
C12 = ((Iyy - Izz) * (thetadot * Cphi * Sphi +
psidot * Sphi ** 2 *Ctheta) +
(Izz - Iyy) * psidot * Cphi ** 2 *Ctheta -
Ixx * psidot * Ctheta)
C13 = (Izz - Iyy) * psidot * Cphi * Sphi * Ctheta ** 2
C21 = ((Izz - Iyy) * (thetadot * Cphi * Sphi +
psidot * Sphi * Ctheta) +
(Iyy - Izz) * psidot * Cphi ** 2 * Ctheta -
Ixx * psidot * Ctheta)
C22 = (Izz - Iyy) * phidot * Cphi * Sphi
C23 = (-Ixx * psidot * Stheta * Ctheta +
Iyy * psidot * Sphi ** 2 * Stheta * Ctheta +
Izz * psidot * Cphi ** 2 * Stheta * Ctheta)
C31 = ((Iyy - Izz) * psidot * Ctheta ** 2 * Sphi * Cphi -
Ixx * thetadot * Ctheta)
C32 = ((Izz - Iyy) * (thetadot * Cphi * Sphi * Stheta +
phidot * Sphi ** 2 * Ctheta) +
(Iyy - Izz) * phidot * Cphi ** 2 * Ctheta +
Ixx * psidot * Stheta * Ctheta -
Iyy * psidot * Sphi ** 2 * Stheta * Ctheta -
Izz * psi * Cphi ** 2 * Stheta * Ctheta)
C33 = ((Iyy - Izz) * phidot * Cphi * Sphi * Ctheta ** 2 -
Iyy * thetadot * Sphi ** 2 * Ctheta * Stheta -
Izz * thetadot * Cphi ** 2 * Ctheta * Stheta +
Ixx * thetadot * Ctheta * Stheta)
C = np.array([[C11,C12,C13],
[C21,C22,C23],
[C31,C32,C33]]);
tau_phi = l * k * (-u[1] ** 2 + u[3] ** 2)
tau_theta = l * k * (-u[0] ** 2 + u[2] ** 2)
tau_psi = b * (u[0] ** 2 - u[1] ** 2 +
u[2] ** 2 - u[3] ** 2)
tau_b = np.array([[tau_phi],[tau_theta],[tau_psi]])
J11 = Ixx
J12 = 0.
J13 = -Ixx*Stheta
J21 = 0.
J22 = Iyy * Cphi ** 2 + Izz * Sphi ** 2
J23 = (Iyy - Izz) * Cphi * Sphi * Ctheta
J31 = -Ixx * Stheta
J32 = (Iyy - Izz) * Cphi * Sphi * Ctheta
J33 = (Ixx * Stheta ** 2 + Iyy * Sphi ** 2 * Ctheta ** 2 +
Izz * Cphi ** 2 * Ctheta ** 2)
J = np.array([[J11,J12,J13],
[J21,J22,J23],
[J31,J32,J33]])
invJ = inv(J)
I3 = np.diag(np.ones([1,3])[0])
Ac = np.zeros([12,12])
Ac[0:3,3:6] = I3
Ac[3:6,3:6] = -(1/m)*np.diag(A)
Ac[6:9,9:12] = I3
Ac[9:12,9:12] = -np.dot(invJ,C)
Rz = (1/m) * np.array([[cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi)],
[sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi)],
[cos(theta) * cos(phi)]])
Bc = np.zeros([12,4])
Bc[3:6,0:1] = Rz
Bc[9:12,1:4] = invJ
Cc = np.zeros([7,12])
Cc[0:7,2:9] = np.diag(np.ones([1,7])[0])
Dc = np.array([])
discreteSystem = cont2discrete((Ac,Bc,Cc,Dc), Ts, method='zoh', alpha=None)
Ad = discreteSystem[0]
Bd = discreteSystem[1]
Cd = discreteSystem[2]
G = np.zeros([12,1])
G[5,0]=-g
# Sets up control signal
u = np.concatenate([[[T]],tau_b])
xout = np.dot(Ad,x) + np.dot(Bd,u) + G*Ts
yout = np.dot(Cd,x)
return xout, yout
def c2d(self, dt):
out = signal.cont2discrete((squeeze(self.num), squeeze(self.den)),dt,self.c2dmethod)
self.numz = squeeze(out[0])
self.denz = squeeze(out[1])
self.Nden = len(self.denz)
