def detectEdge(img):
sobelX = np.array([[1, 0, -1], [2, 0, -2], [1, 0, -1]])
sobelY = sobelX.T
img = c2d(img, sobelX)
img = c2d(img, sobelY)
return img
def test_gbt(self):
ac = np.eye(2)
bc = 0.5 * np.ones((2, 1))
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
dt_requested = 0.5
alpha = 1.0 / 3.0
ad_truth = 1.6 * np.eye(2)
bd_truth = 0.3 * np.ones((2, 1))
cd_truth = np.array([[0.9, 1.2],
[1.2, 1.2],
[1.2, 0.3]])
dd_truth = np.array([[0.175],
[0.2],
[-0.205]])
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested,
method='gbt', alpha=alpha)
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
def __init__(self, params):
# define the state transition and input matrices for SS model
self.A = np.zeros((params.nstates, params.nstates))
self.B = np.zeros((params.nstates, params.ninputs))
self.C = np.zeros( (params.nstates) )
self.D = np.zeros( (params.ninputs) )
self.A[0][3] = 1
self.A[1][4] = 1
self.A[2][5] = 1
self.A[3][3] = -params.kx/params.m
self.A[4][4] = -params.ky/params.m
self.A[5][5] = -params.kz/params.m
self.A[7][6] = 1
self.A[8][2] = 1
self.B[3][0] = 1/params.m
self.B[4][1] = -1/params.m
self.B[5][2] = 1/params.m
self.B[6][3] = 1
# convert the continuous SS model to Discrete SS model
self.Ad, self.Bd, self.Cd, self.Dd, self.dt = c2d((self.A, self.B, self.C, self.D), params.Ts, method='zoh')
#Split the system into two matrices,
# 1 ==> Obstacle area, x(k+1) = I*x(k)
# 2 ==> Normal area, x(k+1) = A*x(k) + Bu()
self.A1 = np.identity(params.nstates)
self.B1 = np.zeros((params.nstates, params.ninputs))
self.A2 = self.Ad
self.B2 = self.Bd
def make_filter_to_size(size):
"""
make a filter mask to size as an image
:param size: the size of desired mask
:return: the filter in the stated size, normalized
"""
# make blur mask filter based on the vector and kernel size
blur_vector = np.matrix(np.array([1, 1]))
blur_mask = c2d(blur_vector, blur_vector) # initial mask
for i in range(size - 3): # up-size mask to kernel size
blur_mask = c2d(blur_mask, blur_vector)
# normalize filter
blur_mask = blur_mask / np.sum(blur_mask)
return blur_mask
def test_gbt(self):
ac = np.eye(2)
bc = 0.5 * np.ones((2, 1))
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
dt_requested = 0.5
alpha = 1.0 / 3.0
ad_truth = 1.6 * np.eye(2)
bd_truth = 0.3 * np.ones((2, 1))
cd_truth = np.array([[0.9, 1.2],
[1.2, 1.2],
[1.2, 0.3]])
dd_truth = np.array([[0.175],
[0.2],
[-0.205]])
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested,
method='gbt', alpha=alpha)
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
def generate(self, image, layer_definitions=[], pad=0):
self.max_value = 255
self.stack = (image / self.max_value).astype(np.float32)
bands = np.split(np.int32(image), image.shape[2], axis=2)
if image.shape[2] == 3:
self.layers = ['band_1', 'band_2', 'band_3']
else:
self.layers = ['band_1', 'band_2', 'band_3', 'band_4']
for layer_def in layer_definitions:
if layer_def['name'] == 'average':
kernels = layer_def['kernels']
solid_kernel = layer_def['solid_kernel']
for size in range(3, (kernels * 2) + 3, 2):
kernel = np.ones((size, size))
if not solid_kernel:
kernel[1:size - 1, 1:size - 1] = 0
kernel = kernel / np.sum(kernel)
for band in range(image.shape[2]):
b = c2d(image[:, :, band], kernel, mode='same')
b = (b / self.max_value).astype(np.float32)
self.stack = np.dstack((self.stack, b))
self.layers.append('band_{}_avg_{}'.format(band + 1, size))
elif layer_def['name'] == 'gli':
denom = np.clip(2 * bands[1] + bands[0] + bands[2], 1, None)
gli = (((2 * bands[1] - bands[0] - bands[2]) / denom) + 1.0) / 2.0
self.stack = np.dstack((self.stack, gli.astype(np.float32)))
self.layers.append('gli')
elif layer_def['name'] == 'lightness':
maximum = np.maximum(bands[0], bands[1])
maximum = np.maximum(maximum, bands[2])
minimum = np.minimum(bands[0], bands[1])
minimum = np.minimum(minimum, bands[2])
lightness = ((maximum + minimum) / 2) / self.max_value
self.stack = np.dstack((self.stack, lightness.astype(np.float32)))
self.layers.append('lightness')
elif layer_def['name'] == 'luminosity':
luminosity = (0.21 * bands[0] + 0.72 * bands[1] + 0.07 * bands[2]) / self.max_value
self.stack = np.dstack((self.stack, luminosity.astype(np.float32)))
self.layers.append('luminosity')
elif layer_def['name'] == 'rgb_average':
average = ((bands[0] + bands[1] + bands[2]) / 3) / self.max_value
self.stack = np.dstack((self.stack, average.astype(np.float32)))
self.layers.append('rgb_average')
elif layer_def['name'] == 'vari':
denom = bands[1] + bands[0] - bands[2]
denom[denom == 0.0] = 1
vari = (((bands[1] - bands[0]) / denom) + 1.0) / 2.0
self.stack = np.dstack((self.stack, vari.astype(np.float32)))
self.layers.append('vari')
elif layer_def['name'] == 'vndvi':
denom = np.clip(bands[1] + bands[0], 1, None)
vndvi = (((bands[1] - bands[0]) / denom) + 1.0) / 2.0
self.stack = np.dstack((self.stack, vndvi.astype(np.float32)))
self.layers.append('vndvi')
else:
pass
if pad > 0:
self.stack = np.pad(self.stack, ((pad, pad), (pad, pad), (0, 0)), mode='symmetric')
return self.stack, self.layers
def test_simo_tf(self):
# See gh-5753
tf = ([[1, 0], [1, 1]], [1, 1])
num, den, dt = c2d(tf, 0.01)
self.assertEqual(dt, 0.01) # sanity check
assert_allclose(den, [1, -0.990404983], rtol=1e-3)
assert_allclose(num, [[1, -1], [1, -0.99004983]], rtol=1e-3)
def test_simo_tf(self):
# See gh-5753
tf = ([[1, 0], [1, 1]], [1, 1])
num, den, dt = c2d(tf, 0.01)
assert_equal(dt, 0.01) # sanity check
assert_allclose(den, [1, -0.990404983], rtol=1e-3)
assert_allclose(num, [[1, -1], [1, -0.99004983]], rtol=1e-3)
def test_bilinear(self):
ac = np.eye(2)
bc = np.full((2, 1), 0.5)
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
dt_requested = 0.5
ad_truth = (5.0 / 3.0) * np.eye(2)
bd_truth = np.full((2, 1), 1.0 / 3.0)
cd_truth = np.array([[1.0, 4.0 / 3.0], [4.0 / 3.0, 4.0 / 3.0],
[4.0 / 3.0, 1.0 / 3.0]])
dd_truth = np.array([[0.291666666666667], [1.0 / 3.0],
[-0.121666666666667]])
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc),
dt_requested,
method='bilinear')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
# Same continuous system again, but change sampling rate
ad_truth = 1.4 * np.eye(2)
bd_truth = np.full((2, 1), 0.2)
cd_truth = np.array([[0.9, 1.2], [1.2, 1.2], [1.2, 0.3]])
dd_truth = np.array([[0.175], [0.2], [-0.205]])
dt_requested = 1.0 / 3.0
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc),
dt_requested,
method='bilinear')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
def test_bilinear(self):
ac = np.eye(2)
bc = 0.5 * np.ones((2, 1))
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
dt_requested = 0.5
ad_truth = (5.0 / 3.0) * np.eye(2)
bd_truth = (1.0 / 3.0) * np.ones((2, 1))
cd_truth = np.array([[1.0, 4.0 / 3.0],
[4.0 / 3.0, 4.0 / 3.0],
[4.0 / 3.0, 1.0 / 3.0]])
dd_truth = np.array([[0.291666666666667],
[1.0 / 3.0],
[-0.121666666666667]])
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested,
method='bilinear')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
# Same continuous system again, but change sampling rate
ad_truth = 1.4 * np.eye(2)
bd_truth = 0.2 * np.ones((2, 1))
cd_truth = np.array([[0.9, 1.2], [1.2, 1.2], [1.2, 0.3]])
dd_truth = np.array([[0.175], [0.2], [-0.205]])
dt_requested = 1.0 / 3.0
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested,
method='bilinear')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
def test_gbt_with_sio_tf_and_zpk(self):
"""Test method='gbt' with alpha=0.25 for tf and zpk cases."""
# State space coefficients for the continuous SIO system.
A = -1.0
B = 1.0
C = 1.0
D = 0.5
# The continuous transfer function coefficients.
cnum, cden = ss2tf(A, B, C, D)
# Continuous zpk representation
cz, cp, ck = ss2zpk(A, B, C, D)
h = 1.0
alpha = 0.25
# Explicit formulas, in the scalar case.
Ad = (1 + (1 - alpha) * h * A) / (1 - alpha * h * A)
Bd = h * B / (1 - alpha * h * A)
Cd = C / (1 - alpha * h * A)
Dd = D + alpha * C * Bd
# Convert the explicit solution to tf
dnum, dden = ss2tf(Ad, Bd, Cd, Dd)
# Compute the discrete tf using cont2discrete.
c2dnum, c2dden, dt = c2d((cnum, cden), h, method='gbt', alpha=alpha)
assert_allclose(dnum, c2dnum)
assert_allclose(dden, c2dden)
# Convert explicit solution to zpk.
dz, dp, dk = ss2zpk(Ad, Bd, Cd, Dd)
# Compute the discrete zpk using cont2discrete.
c2dz, c2dp, c2dk, dt = c2d((cz, cp, ck), h, method='gbt', alpha=alpha)
assert_allclose(dz, c2dz)
assert_allclose(dp, c2dp)
assert_allclose(dk, c2dk)
def test_gbt_with_sio_tf_and_zpk(self):
"""Test method='gbt' with alpha=0.25 for tf and zpk cases."""
# State space coefficients for the continuous SIO system.
A = -1.0
B = 1.0
C = 1.0
D = 0.5
# The continuous transfer function coefficients.
cnum, cden = ss2tf(A, B, C, D)
# Continuous zpk representation
cz, cp, ck = ss2zpk(A, B, C, D)
h = 1.0
alpha = 0.25
# Explicit formulas, in the scalar case.
Ad = (1 + (1 - alpha) * h * A) / (1 - alpha * h * A)
Bd = h * B / (1 - alpha * h * A)
Cd = C / (1 - alpha * h * A)
Dd = D + alpha * C * Bd
# Convert the explicit solution to tf
dnum, dden = ss2tf(Ad, Bd, Cd, Dd)
# Compute the discrete tf using cont2discrete.
c2dnum, c2dden, dt = c2d((cnum, cden), h, method='gbt', alpha=alpha)
assert_allclose(dnum, c2dnum)
assert_allclose(dden, c2dden)
# Convert explicit solution to zpk.
dz, dp, dk = ss2zpk(Ad, Bd, Cd, Dd)
# Compute the discrete zpk using cont2discrete.
c2dz, c2dp, c2dk, dt = c2d((cz, cp, ck), h, method='gbt', alpha=alpha)
assert_allclose(dz, c2dz)
assert_allclose(dp, c2dp)
assert_allclose(dk, c2dk)
def test_transferfunction(self):
numc = np.array([0.25, 0.25, 0.5])
denc = np.array([0.75, 0.75, 1.0])
numd = np.array([[1.0 / 3.0, -0.427419169438754, 0.221654141101125]])
dend = np.array([1.0, -1.351394049721225, 0.606530659712634])
dt_requested = 0.5
num, den, dt = c2d((numc, denc), dt_requested, method='zoh')
assert_array_almost_equal(numd, num)
assert_array_almost_equal(dend, den)
assert_almost_equal(dt_requested, dt)
def test_multioutput(self):
ts = 0.01 # time step
tf = ([[1, -3], [1, 5]], [1, 1])
num, den, dt = c2d(tf, ts)
tf1 = (tf[0][0], tf[1])
num1, den1, dt1 = c2d(tf1, ts)
tf2 = (tf[0][1], tf[1])
num2, den2, dt2 = c2d(tf2, ts)
# Sanity checks
assert_equal(dt, dt1)
assert_equal(dt, dt2)
# Check that we get the same results
assert_allclose(num, np.vstack((num1, num2)), rtol=1e-13)
# Single input, so the denominator should
# not be multidimensional like the numerator
assert_allclose(den, den1, rtol=1e-13)
assert_allclose(den, den2, rtol=1e-13)
def test_transferfunction(self):
numc = np.array([0.25, 0.25, 0.5])
denc = np.array([0.75, 0.75, 1.0])
numd = np.array([[1.0 / 3.0, -0.427419169438754, 0.221654141101125]])
dend = np.array([1.0, -1.351394049721225, 0.606530659712634])
dt_requested = 0.5
num, den, dt = c2d((numc, denc), dt_requested, method='zoh')
assert_array_almost_equal(numd, num)
assert_array_almost_equal(dend, den)
assert_almost_equal(dt_requested, dt)
def test_multioutput(self):
ts = 0.01 # time step
tf = ([[1, -3], [1, 5]], [1, 1])
num, den, dt = c2d(tf, ts)
tf1 = (tf[0][0], tf[1])
num1, den1, dt1 = c2d(tf1, ts)
tf2 = (tf[0][1], tf[1])
num2, den2, dt2 = c2d(tf2, ts)
# Sanity checks
self.assertEqual(dt, dt1)
self.assertEqual(dt, dt2)
# Check that we get the same results
assert_allclose(num, np.vstack((num1, num2)), rtol=1e-13)
# Single input, so the denominator should
# not be multidimensional like the numerator
assert_allclose(den, den1, rtol=1e-13)
assert_allclose(den, den2, rtol=1e-13)
def test_zerospolesgain(self):
zeros_c = np.array([0.5, -0.5])
poles_c = np.array([1.0j / np.sqrt(2), -1.0j / np.sqrt(2)])
k_c = 1.0
zeros_d = [1.23371727305860, 0.735356894461267]
polls_d = [0.938148335039729 + 0.346233593780536j, 0.938148335039729 - 0.346233593780536j]
k_d = 1.0
dt_requested = 0.5
zeros, poles, k, dt = c2d((zeros_c, poles_c, k_c), dt_requested, method="zoh")
assert_array_almost_equal(zeros_d, zeros)
assert_array_almost_equal(polls_d, poles)
assert_almost_equal(k_d, k)
assert_almost_equal(dt_requested, dt)
def test_zoh(self):
ac = np.eye(2)
bc = np.full((2, 1), 0.5)
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
ad_truth = 1.648721270700128 * np.eye(2)
bd_truth = np.full((2, 1), 0.324360635350064)
# c and d in discrete should be equal to their continuous counterparts
dt_requested = 0.5
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested, method='zoh')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cc, cd)
assert_array_almost_equal(dc, dd)
assert_almost_equal(dt_requested, dt)
def test_zoh(self):
ac = np.eye(2)
bc = 0.5 * np.ones((2, 1))
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
ad_truth = 1.648721270700128 * np.eye(2)
bd_truth = 0.324360635350064 * np.ones((2, 1))
# c and d in discrete should be equal to their continuous counterparts
dt_requested = 0.5
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested, method='zoh')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cc, cd)
assert_array_almost_equal(dc, dd)
assert_almost_equal(dt_requested, dt)
def test_zerospolesgain(self):
zeros_c = np.array([0.5, -0.5])
poles_c = np.array([1.j / np.sqrt(2), -1.j / np.sqrt(2)])
k_c = 1.0
zeros_d = [1.23371727305860, 0.735356894461267]
polls_d = [0.938148335039729 + 0.346233593780536j,
0.938148335039729 - 0.346233593780536j]
k_d = 1.0
dt_requested = 0.5
zeros, poles, k, dt = c2d((zeros_c, poles_c, k_c), dt_requested,
method='zoh')
assert_array_almost_equal(zeros_d, zeros)
assert_array_almost_equal(polls_d, poles)
assert_almost_equal(k_d, k)
assert_almost_equal(dt_requested, dt)
def test_backward_diff(self):
ac = np.eye(2)
bc = 0.5 * np.ones((2, 1))
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
dt_requested = 0.5
ad_truth = 2.0 * np.eye(2)
bd_truth = 0.5 * np.ones((2, 1))
cd_truth = np.array([[1.5, 2.0], [2.0, 2.0], [2.0, 0.5]])
dd_truth = np.array([[0.875], [1.0], [0.295]])
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested, method="backward_diff")
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
def test_discrete_approx(self):
"""
Test that the solution to the discrete approximation of a continuous
system actually approximates the solution to the continuous system.
This is an indirect test of the correctness of the implementation
of cont2discrete.
"""
def u(t):
return np.sin(2.5 * t)
a = np.array([[-0.01]])
b = np.array([[1.0]])
c = np.array([[1.0]])
d = np.array([[0.2]])
x0 = 1.0
t = np.linspace(0, 10.0, 101)
dt = t[1] - t[0]
u1 = u(t)
# Use lsim2 to compute the solution to the continuous system.
t, yout, xout = lsim2((a, b, c, d),
T=t,
U=u1,
X0=x0,
rtol=1e-9,
atol=1e-11)
# Convert the continuous system to a discrete approximation.
dsys = c2d((a, b, c, d), dt, method='bilinear')
# Use dlsim with the pairwise averaged input to compute the output
# of the discrete system.
u2 = 0.5 * (u1[:-1] + u1[1:])
t2 = t[:-1]
td2, yd2, xd2 = dlsim(dsys, u=u2.reshape(-1, 1), t=t2, x0=x0)
# ymid is the average of consecutive terms of the "exact" output
# computed by lsim2. This is what the discrete approximation
# actually approximates.
ymid = 0.5 * (yout[:-1] + yout[1:])
assert_allclose(yd2.ravel(), ymid, rtol=1e-4)
def velocity_profile(self,
profile='uniform',
attrs={'scalar': 1.},
norm=True,
scale=False,
conv=None):
"""
Generate either uniform or harmonic profile perscribed by attrs
example- 'harmonic', {'scalar':3., 'xf':2., 'yf':2., 'xa':1.,'ya':1.}
:param str profile: currently supports 'uniform' and 'harmonic'
:param dict attrs: attributes of profile
:param bool norm: normalize velocity Vmax := 1
:param bool scale: scale velocity by std
:param array conv: INDEV: convolution kernel for vectorized\
velocity fields
"""
if profile is 'uniform':
self.velocity = attrs['scalar'] * np.ones(self.meshSize)
if profile is 'harmonic':
print 'harmonic'
self.L = self.x.shape
self.velocity = attrs['scalar'] +\
(attrs['xa'] * np.sin(np.pi*attrs['xf'] * np.pi *
self.x/self.L[1]) *\
attrs['ya'] * np.sin(np.pi*attrs['yf'] * np.pi *
self.y/self.L[0])
)
if conv:
self.conv_vel = self.velocity
kernel = conv['kernel']
for _ in conv['iter']:
self.conv_vel = c2d(kernel, self.conv_vel)
if norm:
self.velocity /= np.max(self.velocity)
elif scale:
self.velocity /= np.std(self.velocity)
self.vmax = np.max(self.velocity)
def test_euler(self):
ac = np.eye(2)
bc = 0.5 * np.ones((2, 1))
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
dt_requested = 0.5
ad_truth = 1.5 * np.eye(2)
bd_truth = 0.25 * np.ones((2, 1))
cd_truth = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dd_truth = dc
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested, method="euler")
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
def test_backward_diff(self):
ac = np.eye(2)
bc = np.full((2, 1), 0.5)
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
dt_requested = 0.5
ad_truth = 2.0 * np.eye(2)
bd_truth = np.full((2, 1), 0.5)
cd_truth = np.array([[1.5, 2.0], [2.0, 2.0], [2.0, 0.5]])
dd_truth = np.array([[0.875], [1.0], [0.295]])
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc),
dt_requested,
method='backward_diff')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
def test_impulse(self):
ac = np.eye(2)
bc = 0.5 * np.ones((2, 1))
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [0.0]])
# True values are verified with Matlab
ad_truth = 1.648721270700128 * np.eye(2)
bd_truth = 0.412180317675032 * np.ones((2, 1))
cd_truth = cc
dd_truth = np.array([[0.4375], [0.5], [0.3125]])
dt_requested = 0.5
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested,
method='impulse')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
def test_impulse(self):
ac = np.eye(2)
bc = np.full((2, 1), 0.5)
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [0.0]])
# True values are verified with Matlab
ad_truth = 1.648721270700128 * np.eye(2)
bd_truth = np.full((2, 1), 0.412180317675032)
cd_truth = cc
dd_truth = np.array([[0.4375], [0.5], [0.3125]])
dt_requested = 0.5
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested,
method='impulse')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
def test_foh(self):
ac = np.eye(2)
bc = np.full((2, 1), 0.5)
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
# True values are verified with Matlab
ad_truth = 1.648721270700128 * np.eye(2)
bd_truth = np.full((2, 1), 0.420839287058789)
cd_truth = cc
dd_truth = np.array([[0.260262223725224], [0.297442541400256],
[-0.144098411624840]])
dt_requested = 0.5
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested, method='foh')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
def test_discrete_approx(self):
"""
Test that the solution to the discrete approximation of a continuous
system actually approximates the solution to the continuous sytem.
This is an indirect test of the correctness of the implementation
of cont2discrete.
"""
def u(t):
return np.sin(2.5 * t)
a = np.array([[-0.01]])
b = np.array([[1.0]])
c = np.array([[1.0]])
d = np.array([[0.2]])
x0 = 1.0
t = np.linspace(0, 10.0, 101)
dt = t[1] - t[0]
u1 = u(t)
# Use lsim2 to compute the solution to the continuous system.
t, yout, xout = lsim2((a, b, c, d), T=t, U=u1, X0=x0,
rtol=1e-9, atol=1e-11)
# Convert the continuous system to a discrete approximation.
dsys = c2d((a, b, c, d), dt, method='bilinear')
# Use dlsim with the pairwise averaged input to compute the output
# of the discrete system.
u2 = 0.5 * (u1[:-1] + u1[1:])
t2 = t[:-1]
td2, yd2, xd2 = dlsim(dsys, u=u2.reshape(-1, 1), t=t2, x0=x0)
# ymid is the average of consecutive terms of the "exact" output
# computed by lsim2. This is what the discrete approximation
# actually approximates.
ymid = 0.5 * (yout[:-1] + yout[1:])
assert_allclose(yd2.ravel(), ymid, rtol=1e-4)
def test_foh(self):
ac = np.eye(2)
bc = 0.5 * np.ones((2, 1))
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
# True values are verified with Matlab
ad_truth = 1.648721270700128 * np.eye(2)
bd_truth = 0.420839287058789 * np.ones((2, 1))
cd_truth = cc
dd_truth = np.array([[0.260262223725224],
[0.297442541400256],
[-0.144098411624840]])
dt_requested = 0.5
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc), dt_requested, method='foh')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
def test_euler(self):
ac = np.eye(2)
bc = np.full((2, 1), 0.5)
cc = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dc = np.array([[0.0], [0.0], [-0.33]])
dt_requested = 0.5
ad_truth = 1.5 * np.eye(2)
bd_truth = np.full((2, 1), 0.25)
cd_truth = np.array([[0.75, 1.0], [1.0, 1.0], [1.0, 0.25]])
dd_truth = dc
ad, bd, cd, dd, dt = c2d((ac, bc, cc, dc),
dt_requested,
method='euler')
assert_array_almost_equal(ad_truth, ad)
assert_array_almost_equal(bd_truth, bd)
assert_array_almost_equal(cd_truth, cd)
assert_array_almost_equal(dd_truth, dd)
assert_almost_equal(dt_requested, dt)
def test_step_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont = step2(sys, T=time, **self.tolerances)
_, yout_disc = dstep(c2d(sys, sample_time, method='zoh'), n=len(time))
assert_allclose(yout_cont.ravel(), yout_disc[0].ravel())
sobel = Sobel(ctx, queue)
sobel(in_buf, imgx_buf, imgy_buf, mag_buf)
print(imgx_buf.get())
print(mag_buf.get())
# Test the conv
#conv = NaiveSeparableCorrelation(ctx, queue)
conv = LocalMemorySeparableCorrelation(ctx, queue)
conv(in_buf, row_buf, col_buf, out_buf)
full_kernel = np.outer(col_k, row_k)
print(full_kernel)
from scipy.signal import correlate2d as c2d
gt = c2d(test_im, full_kernel, mode='same', boundary='symm')
# print "Input: "
# print(test_im)
# print "ground truth"
# print(gt)
# print "cl output"
# print(out_buf.get())
# print "diff"
# print(gt - out_buf.get())
if not np.allclose(gt, out_buf.get()):
plt.imshow(gt - out_buf.get())
def test_impulse_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont = impulse2(sys, T=time, **self.tolerances)
_, yout_disc = dimpulse(c2d(sys, sample_time, method='impulse'),
n=len(time))
assert_allclose(sample_time * yout_cont.ravel(), yout_disc[0].ravel())
def test_linear_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont, _ = lsim2(sys, T=time, U=time, **self.tolerances)
_, yout_disc, _ = dlsim(c2d(sys, sample_time, method='foh'), u=time)
assert_allclose(yout_cont.ravel(), yout_disc.ravel())
def test_step_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont = step2(sys, T=time, **self.tolerances)
_, yout_disc = dstep(c2d(sys, sample_time, method='zoh'), n=len(time))
assert_allclose(yout_cont.ravel(), yout_disc[0].ravel())
def test_impulse_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont = impulse2(sys, T=time, **self.tolerances)
_, yout_disc = dimpulse(c2d(sys, sample_time, method='impulse'),
n=len(time))
assert_allclose(sample_time * yout_cont.ravel(), yout_disc[0].ravel())
def test_linear_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont, _ = lsim2(sys, T=time, U=time, **self.tolerances)
_, yout_disc, _ = dlsim(c2d(sys, sample_time, method='foh'), u=time)
assert_allclose(yout_cont.ravel(), yout_disc.ravel())
def compare_ncc(image1, image2):
image1_n = normalizeForCC(rgb2grey(image1))
image2_n = normalizeForCC(rgb2grey(image2))
# take the mean in case there are suble differences in size
return float((c2d(image1_n, image2_n, 'valid') / image1_n.size).mean())
#BLG 354E 2018 Apr HW-2 Istanbul Technical University
#Yunus Güngör-Student No:150150701
#question 8
import numpy as np
from scipy.signal import convolve2d as c2d
import matplotlib.image as img
#assumed './noisyCameraman.png exists'
#form matrices
K = np.ones((3, 3)) * (1 / 9)
data = img.imread('noisyCameraman.png')
width = len(data[:, 0, 0])
height = len(data[0, :, 0])
resultRed = c2d(data[:, :, 0], K)
resultGreen = c2d(data[:, :, 1], K)
resultBlue = c2d(data[:, :, 2], K)
data[:, :, 0] = resultRed[0:width, 0:height]
data[:, :, 1] = resultGreen[0:width, 0:height]
data[:, :, 2] = resultBlue[0:width, 0:height]
img.imsave('noisyCameramanEdited.png', data)
# Define prediction time step and number of value iterations
dt = 0.005
gamma = 0.7
# Define simulation constants
m = 0.5
M = 0.5
l = 0.5
b = 0.1
g = 9.82
# Build state prediction matrices
A = np.array(
[[0, 1, 0, 0], [0, -4 * b / (4 * M + m), 0, 3 * m * g / (4 * M + m)],
[0, -3 * b / (l * (4 * M + m)), 0, 6 * (m + M) * g / (l * (4 * M + m))],
[0, 0, 1, 0]])
B = np.array([[0, 4 / (4 * M + m), 0, 3 / (l * (4 * M + m))]])
B.shape = (4, 1)
C = np.identity(4)
D = np.zeros((4, 1))
Ad, Bd, Cd, Dd, dt_act = c2d((A, B, C, D), dt, method='zoh')
# Save parameters to file
params = Params(theta_span, thetad_span, x_span, v_span, a_span, dt, Ad, Bd,
gamma)
f = open('trained_data/params.p', 'wb')
pickle.dump(params, f)
f.close()
sobel = Sobel(ctx, queue)
sobel(in_buf, imgx_buf, imgy_buf, mag_buf)
print(imgx_buf.get())
print(mag_buf.get())
# Test the conv
#conv = NaiveSeparableCorrelation(ctx, queue)
conv = LocalMemorySeparableCorrelation(ctx, queue)
conv(in_buf, row_buf, col_buf, out_buf)
full_kernel = np.outer(col_k, row_k)
print(full_kernel)
from scipy.signal import correlate2d as c2d
gt = c2d(test_im, full_kernel, mode='same', boundary='symm')
# print "Input: "
# print(test_im)
# print "ground truth"
# print(gt)
# print "cl output"
# print(out_buf.get())
# print "diff"
# print(gt - out_buf.get())
if not np.allclose(gt, out_buf.get()):
plt.imshow(gt - out_buf.get())