def FundamentalMatrix(self, K1, K2, E):
"""Calculate the Fundamental Matrix."""
# <013> Estimate manually the fundamental matrix.
K2_new = linalg.transpose(linalg.inv(K2))
K1_new = linalg.inv(K1)
F = K2_new * E * K1_new
return F
def mahalanobisDistance(color, avarage, covariance):
# mahalanobis distance: sqrt((x - y)^T * cov^-1 * (x - y))
# x: color
# y: avarage
# linalg: linear algebra
# d1: x - y * cov^-1
d1 = (color - avarage.dot(linalg.inv(covariance)))
# d2: (x -y)^T
d2 = d1.dot((color - avarage).T)
d3 = sqrt(d2)
return d3
def fit_allgasdata( E, conservative=True ) : A = [] # design matrix Y = [] # target values for the fit for e in E : for d in e['data'] : A.append( [ e['D47eq']/d['sD47'], d['d47']/d['sD47'], 1./d['sD47'] ] ) Y.append( d['D47'] / d['sD47'] ) A,Y = numpy.array(A), numpy.array(Y) f = linalg.lstsq(A,Y.T)[0] # best-fit parameters CM = linalg.inv(linalg.dot(A.T,A)) # covariance matrix of fit parameters if conservative : # Scale up uncertainties in the fit parameters if the goodnes-of-fit is worse than average. # To some extent, this helps account for external errors in the gas line data. chi2 = sum( ( Y - linalg.dot( A, f ) )**2 ) nf = sum([len(e['data']) for e in E]) -3 if chi2 > nf : CM = CM * chi2 / nf return f,CM
def fit_gaslines( E, conservative=True ) : A = [] # design matrix Y = [] # target values for the fit nE = len(E) N = sum([len(e['data']) for e in E]) r = 0 for e in E : v = [0.]*nE v[r] = 1. r = r + 1 for d in e['data'] : A.append( scipy.array( [ d['d47'] ] + v ) / d['sD47'] ) Y.append( d['D47'] / d['sD47'] ) A,Y = scipy.array(A), scipy.array(Y) f = linalg.lstsq(A,Y.T)[0] # best-fit parameters C = linalg.inv(linalg.dot(A.T,A)) # covariance matrix of fit parameters if conservative : # Scale up uncertainties in the fit parameters if the goodnes-of-fit is worse than average. # To some extent, this helps account for external errors in the gas line data. chi2 = sum( ( Y - linalg.dot( A, f ) )**2 ) nf = N-3 if chi2 > nf : C = C * chi2 / nf return f,C
velY = cannon_ball.GetYVelocity ()
noiseX = cannon_ball.GetXWithNoise ()
noiseY = cannon_ball.GetYWithNoise ()
truth.append ([realX, realY])
measurements.append ([noiseX, noiseY])
cannon_ball.Step ()
estimations.append ([xn.item (0), xn.item(2)])
zn = matrix([[noiseX], [cannon_ball.GetXVelocity ()], [noiseY], [cannon_ball.GetYVelocity ()]])
xn = A * xn + B * un
pn = (A * pn) * A.transpose () + Q
y = zn - H * xn
S = H * pn * H.transpose () + R
K = pn * H.transpose () * linalg.inv (S)
xn = xn + K * y
pn = (I - K * H) * pn
measurements = array (measurements)
truth = array (truth)
estimations = array (estimations)
pylab.figure ()
pylab.plot (measurements[0:measure_count, 0], measurements[0:measure_count, 1], color = 'b', linestyle = '--', label = 'measured')
pylab.plot (truth[0:measure_count, 0], truth[0:measure_count, 1], color = 'r', label = 'true')
pylab.plot (estimations[0:measure_count, 0], estimations[0:measure_count, 1], color = 'g', label = 'kalman')
pylab.legend ()
pylab.show ()
def path_3d(pos, loop):
# INPUTS
# pos: A 3xn python array with the required points of the route
# loop: A boolean, true means that the path is cyclic i.e a loop.
# OUPUTS
# X, Y, Z, Tt
# X, Y, Z: Each one a vector representing the coordinates of every point
# in the calculated path
# Tt: A vector with the time information associated with each point
n_points = pos.shape[0]
path_x = 0
path_y = 0
path_z = 0
path_t = 0
# Vector calculation for the path
if loop:
# If the path is a loop
# Start and end is the same
vectors = zeros((n_points + 1, 3))
for i in range(n_points):
if i < n_points - 1:
# All the internal points
vector1 = pos[i, :] - pos[i - 1, :]
vector2 = pos[i + 1, :] - pos[i, :]
v_result = vector1 + vector2
vectors[i, :] = v_result / sqrt(dot(v_result, v_result))
else:
# Careful with the last one
vector1 = pos[i, :] - pos[i - 1, :]
vector2 = pos[0, :] - pos[i, :]
v_result = vector1 + vector2
vectors[i, :] = v_result / sqrt(dot(v_result, v_result))
# We add an additional point to cloe the loop
vectors[n_points, :] = vectors[0, :]
pos = vstack((pos, pos[0]))
n_points += 1
else:
# If points don't form a loop
vectors = zeros((n_points, 3))
for i in range(n_points):
if (i > 0) and (i < n_points - 1):
# All the internal points
vector1 = pos[i, :] - pos[i - 1, :]
vector2 = pos[i + 1, :] - pos[i, :]
v_result = vector1 + vector2
vectors[i, :] = v_result / sqrt(dot(v_result, v_result))
else:
vectors[0, :] = zeros(3)
vectors[n_points - 1, :] = zeros(3)
to = 1
t1 = 10
time_step = 0.05
for i in range(n_points - 1):
# Initial position and velocities
xi = pos[i, 0]
dxi = vectors[i, 0]
yi = pos[i, 1]
dyi = vectors[i, 1]
zi = pos[i, 2]
dzi = vectors[i, 2]
# Final position and velocities
xf = pos[i + 1, 0]
dxf = vectors[i + 1, 0]
yf = pos[i + 1, 1]
dyf = vectors[i + 1, 1]
zf = pos[i + 1, 2]
dzf = vectors[i + 1, 2]
# time vector
t = arange(to, t1, time_step)
# Matrix for the cubic polynomial calculation
m = array([[1, to, to ** 2, to ** 3],
[0, 1, 2 * to, 3 * to ** 2],
[1, t1, t1 ** 2, t1 ** 3],
[0, 1, 2 * t1, 3 * t1 ** 2]])
m_a = dot(linalg.inv(m), array([[xi], [dxi], [xf], [dxf]]))
m_b = dot(linalg.inv(m), array([[yi], [dyi], [yf], [dyf]]))
m_c = dot(linalg.inv(m), array([[zi], [dzi], [zf], [dzf]]))
x_temp = m_a[0] + m_a[1] * t + m_a[2] * t ** 2 + m_a[3] * t ** 3
y_temp = m_b[0] + m_b[1] * t + m_b[2] * t ** 2 + m_b[3] * t ** 3
z_temp = m_c[0] + m_c[1] * t + m_c[2] * t ** 2 + m_c[3] * t ** 3
if i == 0:
path_x = x_temp
path_y = y_temp
path_z = z_temp
path_t = t
else:
path_x = hstack((path_x, x_temp))
path_y = hstack((path_y, y_temp))
path_z = hstack((path_z, z_temp))
path_t = hstack((path_t, t))
to += 10
t1 += 10
return path_x, path_y, path_z, path_t
def path_3d(pos, loop):
# INPUTS
# pos: A 3xn python array with the required points of the route
# loop: A boolean, true means that the path is cyclic i.e a loop.
# OUPUTS
# X, Y, Z, Tt
# X, Y, Z: Each one a vector representing the coordinates of every point
# in the calculated path
# Tt: A vector with the time information associated with each point
n_points = pos.shape[0]
path_x = 0
path_y = 0
path_z = 0
path_t = 0
# Vector calculation for the path
if loop:
# If the path is a loop
# Start and end is the same
vectors = zeros((n_points + 1, 3))
for i in range(n_points):
if i < n_points - 1:
# All the internal points
vector1 = pos[i, :] - pos[i - 1, :]
vector2 = pos[i + 1, :] - pos[i, :]
v_result = vector1 + vector2
vectors[i, :] = v_result / sqrt(dot(v_result, v_result))
else:
# Careful with the last one
vector1 = pos[i, :] - pos[i - 1, :]
vector2 = pos[0, :] - pos[i, :]
v_result = vector1 + vector2
vectors[i, :] = v_result / sqrt(dot(v_result, v_result))
# We add an additional point to cloe the loop
vectors[n_points, :] = vectors[0, :]
pos = vstack((pos, pos[0]))
n_points += 1
else:
# If points don't form a loop
vectors = zeros((n_points, 3))
for i in range(n_points):
if (i > 0) and (i < n_points - 1):
# All the internal points
vector1 = pos[i, :] - pos[i - 1, :]
vector2 = pos[i + 1, :] - pos[i, :]
v_result = vector1 + vector2
vectors[i, :] = v_result / sqrt(dot(v_result, v_result))
else:
vectors[0, :] = zeros(3)
vectors[n_points - 1, :] = zeros(3)
to = 1
t1 = 10
time_step = 0.05
for i in range(n_points - 1):
# Initial position and velocities
xi = pos[i, 0]
dxi = vectors[i, 0]
yi = pos[i, 1]
dyi = vectors[i, 1]
zi = pos[i, 2]
dzi = vectors[i, 2]
# Final position and velocities
xf = pos[i + 1, 0]
dxf = vectors[i + 1, 0]
yf = pos[i + 1, 1]
dyf = vectors[i + 1, 1]
zf = pos[i + 1, 2]
dzf = vectors[i + 1, 2]
# time vector
t = arange(to, t1, time_step)
# Matrix for the cubic polynomial calculation
m = array([[1, to, to**2, to**3], [0, 1, 2 * to, 3 * to**2],
[1, t1, t1**2, t1**3], [0, 1, 2 * t1, 3 * t1**2]])
m_a = dot(linalg.inv(m), array([[xi], [dxi], [xf], [dxf]]))
m_b = dot(linalg.inv(m), array([[yi], [dyi], [yf], [dyf]]))
m_c = dot(linalg.inv(m), array([[zi], [dzi], [zf], [dzf]]))
x_temp = m_a[0] + m_a[1] * t + m_a[2] * t**2 + m_a[3] * t**3
y_temp = m_b[0] + m_b[1] * t + m_b[2] * t**2 + m_b[3] * t**3
z_temp = m_c[0] + m_c[1] * t + m_c[2] * t**2 + m_c[3] * t**3
if i == 0:
path_x = x_temp
path_y = y_temp
path_z = z_temp
path_t = t
else:
path_x = hstack((path_x, x_temp))
path_y = hstack((path_y, y_temp))
path_z = hstack((path_z, z_temp))
path_t = hstack((path_t, t))
to += 10
t1 += 10
return path_x, path_y, path_z, path_t
truth.append([realX, realY])
measurements.append([noiseX, noiseY])
cannon_ball.Step()
estimations.append([xn.item(0), xn.item(2)])
zn = matrix([[noiseX], [cannon_ball.GetXVelocity()], [noiseY],
[cannon_ball.GetYVelocity()]])
xn = A * xn + B * un
pn = (A * pn) * A.transpose() + Q
y = zn - H * xn
S = H * pn * H.transpose() + R
K = pn * H.transpose() * linalg.inv(S)
xn = xn + K * y
pn = (I - K * H) * pn
measurements = array(measurements)
truth = array(truth)
estimations = array(estimations)
pylab.figure()
pylab.plot(measurements[0:measure_count, 0],
measurements[0:measure_count, 1],
color='b',
linestyle='--',
label='measured')
pylab.plot(truth[0:measure_count, 0],
truth[0:measure_count, 1],
color='r',
