def robot_F_fn(state, dt=1.0):
"""
Transition matrix for robot dynamics
Linearize dynamics about 'state' for EKF
xdot = v*cos(theta+w)
ydot = v*sin(theta+w)
thetadot = w
vdot = 0 -> step size
wdot = 0 -> turn angle per step
"""
x = state.value[0][0]
y = state.value[1][0]
theta = state.value[2][0]
v = state.value[3][0]
w = state.value[4][0]
J = matrix([
[0., 0., -v * sin(theta), cos(theta), 0.],
[0., 0., v * cos(theta), sin(theta), 0.],
[0., 0., 0., 0., 1.],
[0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0.],
])
I = matrix([[]])
I.identity(5)
return I + J * dt
def implement(self):
_x_ = sum(self.xs) / len(self.xs)
_y_ = sum(self.ys) / len(self.ys)
a, b = [], []
for i in range(len(self.xs)):
a.append(self.xs[i] - _x_)
b.append(self.ys[i] - _y_)
dic = {"a": a, "b": b}
ab, aa, bb, aab, abb, aaa, bbb = [], [], [], [], [], [], []
itemlist = ['ab', 'aa', 'bb', 'aab', 'abb', 'aaa', 'bbb']
varlist = [ab, aa, bb, aab, abb, aaa, bbb]
for k in range(len(itemlist)):
t1 = list(itemlist[k])
for i in range(len(self.xs)):
x = 1
for key in t1:
x *= dic[key][i]
# exec "%s.append(%s)" % (itemlist[k], x)
varlist[k].append(x)
M = matrix([[sum(aa), sum(ab)], [sum(ab), sum(bb)]
]).inverse() * matrix([[sum(aaa) + sum(abb)],
[sum(bbb) + sum(aab)]])
dce = [
sum(self.xs) / len(self.xs) + M.value[0][0] / 2.0,
sum(self.ys) / len(self.ys) + M.value[1][0] / 2.0
]
return dce
def setup_kalman_filter():
"""
Setup Kalman Filter for this problem
z : Initial measurement
"""
# Setup 5D Kalman filter
# initial uncertainty: 0 for positions x and y,
# 1000 for the two velocities and accelerations
# measurement function: reflect the fact that
# we observe x and y
H = matrix([[1., 0., 0., 0., 0.], [0., 1., 0., 0., 0.]])
# measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal
R = matrix([[0.1, 0.0], [0.0, 0.1]])
u = matrix([[0.], [0.], [0.], [0.], [0.]]) # external motion
I = identity_matrix(5)
# P = I*10000.0
P = matrix([
[1000.0, 100., 1000., 1000., 0.],
[100., 1000., 1000., 1000., 0.],
[0., 0., 1000., 0., 1000.],
[0., 0., 0., 1000., 0.],
[0., 0., 0., 0., 1000.],
])
# I*1000.0 # 1000 along main diagonal
# P.value[0][0] = 100.0
# P.value[1][1] = 100.0
# P.value[2][2] = 100.0
# P.value[3][3] = 100.0
# P.value[4][4] = 100.0
return [u, P, H, R]
def predict(x, P):
# prediction
# pull out current estimates based on measurement
# this was a big part of what was hainging me up (I was using older estimates before)
x0 = x.value[0][0]
y0 = x.value[1][0]
theta0 = x.value[2][0]
dtheta0 = x.value[3][0]
dist0 = x.value[4][0]
# next state function:
# this is now the Jacobian of the transition matrix (F) from the regular Kalman Filter
A = matrix([[
1.0, 0.0, -dist0 * sin(theta0 + dtheta0),
-dist0 * sin(theta0 + dtheta0),
cos(theta0 + dtheta0)
],
[
0.0, 1.0, dist0 * cos(theta0 + dtheta0),
dist0 * cos(theta0 + dtheta0),
sin(theta0 + dtheta0)
], [0.0, 0.0, 1.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 1.0]])
# calculate new estimate
# it's NOT simply the matrix multiplication of transition matrix and estimated state vector
# for the EKF just use the state transition formulas the transition matrix was built from
x = matrix([[x0 + dist0 * cos(theta0 + dtheta0)],
[y0 + dist0 * sin(theta0 + dtheta0)],
[angle_trunc(theta0 + dtheta0)], [dtheta0], [dist0]])
P = A * P * A.transpose()
return x, P
def jacobi_cycle(A: matrix, prec: float):
assert (A.size1 == A.size2)
V = matrix(A.size1, A.size2)
J = matrix(A.size1, A.size2)
for ii in range(J.size1):
J[ii, ii] = 1
V[ii, ii] = 1
while max([
max([abs(A[i, j]) for j in range(i + 1, A.size1)])
for i in range(0, A.size2 - 1)
]) > prec:
for pp in range(0, A.size1 - 1):
for qq in range(pp + 1, A.size2):
phi = 0.5 * np.arctan2(2.0 * A[pp, qq], A[qq, qq] - A[pp, pp])
J[pp, pp] = cos(phi)
J[qq, qq] = cos(phi)
J[pp, qq] = sin(phi)
J[qq, pp] = -sin(phi)
A = matrix_mult(trans(J), matrix_mult(A, J))
V = matrix_mult(V, J)
J[pp, pp] = 1
J[qq, qq] = 1
J[pp, qq] = 0
J[qq, pp] = 0
return A, V
def __init__(self):
self.a = matrix([[0.], [0.]]) # external motion
self.F = matrix([[1.0, 0.0], [0.0, 1.0]])
self.H = matrix([[1.0, 0.0], [0.0, 1.0]])
self.R = matrix([[0.1, 0.0], [0.0, 0.1]])
self.I = matrix([[1.0, 0.0], [0.0, 1.0]])
def find_ambush_point(hunter_position, robotpos, max_distance, OTHER):
print 'Planning ambush..'
OTHER['chase_counter'] = 0
target_steps = 1
next_xy = robotpos
X = OTHER['X']
P = OTHER['P']
X_copy = matrix([[v for v in c] for c in X.value])
P_copy = matrix([[v for v in c] for c in P.value])
OTHER2 = {'X': X_copy, 'P': P_copy}
while True:
if distance_between(hunter_position,
next_xy) / max_distance < target_steps:
next_xy2, OTHER2 = estimate_next_pos(next_xy, OTHER2)
next_xy = [(next_xy[0] + next_xy2[0]) / 2,
(next_xy[1] + next_xy2[1]) / 2]
break
else:
next_xy, OTHER2 = estimate_next_pos(next_xy, OTHER2)
target_steps += 1
return next_xy
def estimate_next_pos(measurement, OTHER=None):
"""Estimate the next (x, y) position of the wandering Traxbot
based on noisy (x, y) measurements."""
x = matrix([[0.0], [0.0], [0.0], [0.0],
[0.0]]) # initial state (x, y, theta, dtheta, d)
P = matrix([[1000.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1000.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1000.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1000.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 1000.0]])
# initial uncertainty: 1000 for each state variable
xy_estimate = []
state = OTHER
if not OTHER: # this is the first measurement
# using initial guesses
x, P = kalman_filter(x, P, measurement)
state = trackstate(x, P)
xy_estimate = measurement
else:
# measure and then predict
x, P = kalman_filter(state.x, state.P, measurement)
state = trackstate(x, P, OTHER.count + 1)
xy_estimate = x.value[0][0], x.value[1][0]
OTHER = state
OTHER.measurement = measurement
OTHER.estimate = xy_estimate
return xy_estimate, OTHER
def estimate_next_pos(measurement, OTHER=None):
"""Estimate the next (x, y) position of the wandering Traxbot
based on noisy (x, y) measurements."""
if OTHER:
OTHER.append(measurement)
if len(OTHER) < 3:
xy_estimate = measurement
else:
center, radius = saijien(OTHER)
ang = 0.0
for i in range(len(OTHER) - 1):
ang += angle_between(center, radius, OTHER[i], OTHER[i + 1])
ang_mean = ang / (len(OTHER) - 1)
estimate = matrix([[cos(ang_mean), -sin(ang_mean)],
[sin(ang_mean), cos(ang_mean)]]) *\
matrix([[radius/distance_between(measurement,center),0],
[0,radius/distance_between(measurement,center)]])*matrix([[measurement[0]-center[0]],
[measurement[1]-center[1]]]) +\
matrix([[center[0]],
[center[1]]])
xy_estimate = (estimate.value[0][0], estimate.value[1][0])
else:
OTHER = [measurement]
xy_estimate = measurement
# You must return xy_estimate (x, y), and OTHER (even if it is None)
# in this order for grading purposes.
# xy_estimate = (3.2, 9.1)
return xy_estimate, OTHER
def __init__(self, sigma):
# State matrix
self.x = matrix([[0.], # x
[0.], # y
[0.], # heading
[0.], # turning
[0.]]) # distance
# Covariance matrix
self.P = matrix([[1000., 0., 0., 0., 0.],
[0., 1000., 0., 0., 0.],
[0., 0., 1000., 0., 0.],
[0., 0., 0., 1000., 0.],
[0., 0., 0., 0., 1000.]])
# measurement uncertainty
self.R = matrix([[sigma, 0.],
[0., sigma]])
# transition matrix
self.F = ExtendedKalmanFilter.transitionMatrix(self.x)
# measurement function
self.H = matrix([[1., 0., 0., 0., 0.],
[0., 1., 0., 0., 0.]])
# identity matrix
self.I = matrix([[1., 0., 0., 0., 0.],
[0., 1., 0., 0., 0.],
[0., 0., 1., 0., 0.],
[0., 0., 0., 1., 0.],
[0., 0., 0., 0., 1.]])
self.keep = []
def estimate_next_pos(measurement, OTHER = None):
"""Estimate the next (x, y) position of the wandering Traxbot
based on noisy (x, y) measurements."""
# You must return xy_estimate (x, y), and OTHER (even if it is None)
# in this order for grading purposes.
if OTHER == None:
xy_estimate = measurement
OTHER = [measurement]
else :
if len(OTHER) > 1:
m = len(OTHER) - 1
v1 = [OTHER[m][0] - OTHER[m-1][0], OTHER[m][1] - OTHER[m-1][1]]
v2 = [measurement[0] - OTHER[m][0], measurement[1] - OTHER[m][1]]
dist = distance_between(measurement, OTHER[m])
radian = btwAngle(v1, v2)
sn = sin(radian)
cs = cos(radian)
r = matrix([[cs, -sn], [sn, cs]])
X = matrix([[v2[0]], [v2[1]]])
X = r * X
x = X.value[0][0]
y = X.value[1][0]
v = unitVector([x, y])
xy_estimate = [ measurement[0] + v[0] * dist, measurement[1] + v[1] * dist ]
else:
xy_estimate = measurement
OTHER.append(measurement)
return xy_estimate, OTHER
def robot_F_fn(state, dt = 1.0):
"""
Transition matrix for robot dynamics
Linearize dynamics about 'state' for EKF
xdot = v*cos(theta+w)
ydot = v*sin(theta+w)
thetadot = w
vdot = 0 -> step size
wdot = 0 -> turn angle per step
"""
x = state.value[0][0]
y = state.value[1][0]
theta = state.value[2][0]
v = state.value[3][0]
w = state.value[4][0]
J = matrix([[0., 0., -v*sin(theta), cos(theta), 0.],
[0., 0., v*cos(theta), sin(theta), 0.],
[0., 0., 0., 0., 1.],
[0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0.],
])
I = matrix([[]])
I.identity(5)
return I + J*dt
def estimate_target_position(measurement, OTHER= None):
if OTHER != None:
m_heading, m_distance = convert_measurement_into_polar_coordinates(measurement, OTHER['prev'])
if 'x' not in OTHER.keys():
x = matrix([[m_heading], [m_distance], [0.], [0.]]) # initial state (location and velocity)
P = matrix([[measurement_noise,0,0,0],[0,measurement_noise,0,0],[0,0,1000,0],[0,0,0,1000]])# initial uncertainty: 0 for positions x and y, 1000 for the two velocities
else :
x = OTHER['x']
P = OTHER['P']
while m_heading < (x.value[0][0] - pi): # off by a rotation
m_heading += 2*pi
Z = matrix([[m_heading, m_distance]])
x, P = kalman_prediction(x, P, Z)
heading = x.value[0][0]
distance = x.value[1][0]
new_x = distance * cos(heading)
new_y = distance * sin(heading)
xy_estimate = [measurement[0] + new_x, measurement[1] + new_y]
OTHER = {'x': x, 'P': P, 'prev': measurement}
else:
xy_estimate = measurement
OTHER = {'prev': measurement}
return xy_estimate, OTHER
def test():
m1 = matrix(2, 2)
m1.data = [[1, 2], [3, 4]]
m2 = matrix(2, 2)
m2.data = [[2, 0], [3, 1]]
expectedResult1 = [[8, 2], [18, 4]]
result1 = multiply(m1, m2)
if result1.data == expectedResult1:
print("Success")
else:
print("Fail")
m3 = matrix(4, 4)
m3.data = [[1, 2, 3, 4], [6, 7, 8, 9], [11, 12, 13, 14], [16, 17, 18, 19]]
result2 = multiply(m3, m3)
expectedResult2 = [[110, 120, 130, 140], [280, 310, 340, 370], [450, 500, 550, 600], [620, 690, 760, 830]]
if result2.data == expectedResult2:
print("Success")
else:
print("Fail")
m4 = matrix(5, 5)
m4.data = [[1, 2, 3, 4, 5], [6, 7, 8, 9, 10], [11, 12, 13, 14, 15], [16, 17, 18, 19, 20], [21, 22, 23, 24, 25]]
result3 = multiply(m4, m4)
expectedResult3 = [[215, 230, 245, 260, 275], [490, 530, 570, 610, 650], [765, 830, 895, 960, 1025], [1040, 1130, 1220, 1310, 1400], [1315, 1430, 1545, 1660, 1775]]
if result3.data == expectedResult3:
print("Success")
else:
print("Fail")
def estimate_next_pos(measurement, OTHER = None):
"""Estimate the next (x, y) position of the wandering Traxbot
based on noisy (x, y) measurements."""
# You must return xy_estimate (x, y), and OTHER (even if it is None)
# in this order for grading purposes.
# print "measurement: ", measurement
if OTHER != None:
# measurement update
m_heading, m_distance = convert_measurement_into_polar_coordinates(measurement, OTHER['prev'])
if 'x' not in OTHER.keys():
x = matrix([[m_heading], [m_distance], [0.], [0.]]) # initial state (location and velocity)
P = matrix([[measurement_noise,0,0,0],[0,measurement_noise,0,0],[0,0,1000,0],[0,0,0,1000]])# initial uncertainty: 0 for positions x and y, 1000 for the two velocities
else :
x = OTHER['x']
P = OTHER['P']
while m_heading < (x.value[0][0] - pi): # off by a rotation
m_heading += 2*pi
Z = matrix([[m_heading, m_distance]])
x, P = kalman_prediction(x, P, Z)
heading = x.value[0][0]
distance = x.value[1][0]
new_x = distance * cos(heading)
new_y = distance * sin(heading)
xy_estimate = [measurement[0] + new_x, measurement[1] + new_y]
OTHER = {'x': x, 'P': P, 'prev': measurement}
else:
xy_estimate = measurement
OTHER = {'prev': measurement}
return xy_estimate, OTHER
def estimate_next_pos(measurement, OTHER=None):
"""Estimate the next (x, y) position of the wandering Traxbot
based on noisy (x, y) measurements."""
if OTHER is None:
pd = 1000
x = matrix([[0.], [0.], [0.], [0.], [0.], [0.]])
P = matrix([[pd, 0, 0, 0, 0, 0], [0, pd, 0, 0, 0, 0],
[0, 0, pd, 0, 0, 0], [0, 0, 0, pd, 0, 0],
[0, 0, 0, 0, pd, 0], [0, 0, 0, 0, 0, pd]])
else:
[x, P] = OTHER
#print '=== P'
#P.show()
#print 'Measurement = ', measurement
#print 'State (before update) = ', x
[x, P] = update(x, P, measurement)
#print 'State (after update) = ', x
[x, P] = predict(x, P)
#print 'State (after predict) = ', x
xy_estimate = [x.value[0][0], x.value[1][0]]
OTHER = [x, P]
#print ' '
# You must return xy_estimate (x, y), and OTHER (even if it is None)
# in this order for grading purposes.
return xy_estimate, OTHER
def implement(self):
_x_ = sum(self.xs) / len(self.xs)
_y_ = sum(self.ys) / len(self.ys)
a, b = [], []
for i in range(len(self.xs)):
a.append(self.xs[i] - _x_)
b.append(self.ys[i] - _y_)
dic = {"a":a,"b":b}
ab, aa, bb, aab, abb, aaa, bbb = [], [], [], [], [], [], []
itemlist = ['ab', 'aa', 'bb', 'aab', 'abb', 'aaa', 'bbb']
varlist = [ab, aa, bb, aab, abb, aaa, bbb]
for k in range(len(itemlist)):
t1 = list(itemlist[k])
for i in range(len(self.xs)):
x = 1
for key in t1:
x *= dic[key][i]
# exec "%s.append(%s)" % (itemlist[k], x)
# print "aab", aab
varlist[k].append(x)
M = matrix([[sum(aa), sum(ab)], [sum(ab), sum(bb)]]).inverse() * matrix([[sum(aaa) + sum(abb)], [sum(bbb) + sum(aab)]])
dce = [sum(self.xs) / len(self.xs) + M.value[0][0] / 2.0, sum(self.ys) / len(self.ys) + M.value[1][0] / 2.0]
# A = array([[sum(aa), sum(ab)], [sum(ab), sum(bb)]])
# B = array([sum(aaa) + sum(abb), sum(bbb) + sum(aab)])/2
# uc, vc = linalg.solve(A, B)
# xc_1 = _x_ + uc
# yc_1 = _y_ + vc
# dce = [xc_1, yc_1]
# Ri = calc_R(*dce)
# Radius = Ri.mean()
# residu = sum((Ri - Radius)**2)
return dce
def __init__(self, focus):
self.mouse_sensitivity = 10
self.focus = focus
self.perspektív = matrix([[focus, 0, 0], [0, focus, 0], [0, 0, 1]])
self.eltolas = vector([0, 0, 0])
self.forgatas = matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
self.mouse_pos = [0, 0]
self.speed = 0.03
def next_move(hunter_position,
hunter_heading,
target_measurement,
max_distance,
OTHER=None):
# This function will be called after each time the target moves.
if not OTHER: # first time
target_measurements = [target_measurement]
P = matrix(
[[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 1000., 0.],
[0., 0., 0., 1000.]]
) # initial uncertainty: 0 for positions x and y, 1000 for the two velocities
prev_heading = 0.
OTHER = [target_measurements, P, prev_heading]
else:
target_measurements, P, prev_heading = OTHER
#setup initial state
initial_xy = target_measurement
# initial state (location and velocity)
x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]])
#Run kalman filter
target_measurements_input = target_measurements[
-15:] #optimize to address "13 seconds CPU" error on Udacity
x, P = kalman_filter.filter(x, P, target_measurements_input)
filter_xy = (x.value[0][0], x.value[1][0])
#Post-process
prev_target_measurement = target_measurements[-1]
#distance
distance = distance_between(prev_target_measurement, filter_xy)
#heading/turning
heading = get_heading(prev_target_measurement, filter_xy)
turning = heading - prev_heading
r = robot(filter_xy[0], filter_xy[1], heading, turning, distance)
r.set_noise(0, 0, 0)
r.move_in_circle()
target_estimate = (r.x, r.y)
#Hunting time
distance_hunter_target = distance_between(hunter_position, target_estimate)
heading_hunter_target = get_heading(hunter_position, target_estimate)
turning_hunter_target = heading_hunter_target - hunter_heading
#Update OTHER for next call
OTHER[0].append(filter_xy)
OTHER[1] = P
OTHER[2] = heading
# The OTHER variable is a place for you to store any historical information
# about
# the progress of the hunt (or maybe some localization information). Your
# return format
# must be as follows in order to be graded properly.
return turning_hunter_target, distance_hunter_target, OTHER
def doit(initial_pos, move1, move2, dL_1, dL_2, dL_3):
#
x = matrix([[initial_pos - move1 - dL_1], [move1 - move2 - dL_2],
[move2 - dL_3], [dL_1 + dL_3 + dL_2]])
omega = matrix([[3, -1, 0, -1], [-1, 3, -1, -1], [0, -1, 2, -1],
[-1, -1, -1, 3]])
mu = omega.inverse() * x
#
return mu
def next_move(hunter_position, hunter_heading, target_measurement, max_distance, OTHER = None):
# This function will be called after each time the target moves.
# The OTHER variable is a place for you to store any historical information about
# the progress of the hunt (or maybe some localization information). Your return format
# must be as follows in order to be graded properly.
if OTHER is None:
OTHER = [[],[],[]]
OTHER[0] = [target_measurement]
OTHER[1] = matrix([[0.,0.,0.,0.],[0.,0.,0.,0.],[0.,0.,1000.,0.],[0.,0.,0.,1000.]])
OTHER[2] = 0
# d = sqrt((target_measurement[0]) ** 2 + (target_measurement[1]) ** 2)
# OTHER[3] = [d]
turning = get_heading(hunter_position, target_measurement) - hunter_heading
distance = distance_between(hunter_position, target_measurement)
else:
#implement the kalmen filter for the target measurements
filtered = OTHER[0]
theta = OTHER[2]
_xy = OTHER[0][-1]
x = matrix([[target_measurement[0]], [target_measurement[1]], [0.], [0.]])
cov = OTHER[1]
x, cov = kfilter(filtered[-20:], x, cov)
#based on the filtered current target value to calculate the steering and distance
fx = x.value[0][0]
fy = x.value[1][0]
xy = (fx, fy)
# _x = OTHER[0][-1][0]
# _y = OTHER[0][-1][1]
# _xy = (_x, _y)
d = distance_between(_xy, xy)
# distance_mean = sum(OTHER[3])/len(OTHER[3])
beta = atan2(fy - _xy[1], fx - _xy[0])
steering = beta - theta
#estimate the future position
beta_ = beta + steering
x_ = fx + d * cos(beta_)
y_ = fy + d * sin(beta_)
#based on the future position, calculate the turning and distance
xy_ = (x_, y_)
heading_ = get_heading(hunter_position, xy_)
turning = angle_trunc(heading_ - hunter_heading)
distance = distance_between(hunter_position, xy_)
filtered.append(xy)
#update the OTHER
OTHER[0] = filtered
OTHER[1] = cov
OTHER[2] = beta
# OTHER[3].append(d)
return turning, distance, OTHER
def slam(data, N, num_landmarks, motion_noise, measurement_noise):
# Set the dimension of the filter
dim = 2 * (N + num_landmarks)
# make the constraint information matrix and vector
Omega = matrix()
Omega.zero(dim, dim)
Omega.value[0][0] = 1.0
Omega.value[1][1] = 1.0
Xi = matrix()
Xi.zero(dim, 1)
Xi.value[0][0] = world_size / 2.0
Xi.value[1][0] = world_size / 2.0
# process the data
for k in range(len(data)):
# n is the index of the robot pose in the matrix/vector
n = k * 2
measurement = data[k][0]
motion = data[k][1]
# integrate the measurements
for i in range(len(measurement)):
# m is the index of the landmark coordinate in the matrix/vector
m = 2 * (N + measurement[i][0])
# update the information maxtrix/vector based on the measurement
for b in range(2):
Omega.value[n + b][n + b] += 1.0 / measurement_noise
Omega.value[m + b][m + b] += 1.0 / measurement_noise
Omega.value[n + b][m + b] += -1.0 / measurement_noise
Omega.value[m + b][n + b] += -1.0 / measurement_noise
Xi.value[n +
b][0] += -measurement[i][1 + b] / measurement_noise
Xi.value[m + b][0] += measurement[i][1 + b] / measurement_noise
# update the information maxtrix/vector based on the robot motion
for b in range(4):
Omega.value[n + b][n + b] += 1.0 / motion_noise
for b in range(2):
Omega.value[n + b][n + b + 2] += -1.0 / motion_noise
Omega.value[n + b + 2][n + b] += -1.0 / motion_noise
Xi.value[n + b][0] += -motion[b] / motion_noise
Xi.value[n + b + 2][0] += motion[b] / motion_noise
# compute best estimate
mu = Omega.inverse() * Xi
# return the result
return mu
def slam(data, N, num_landmarks, motion_noise, measurement_noise):
# Set the dimension of the filter
dim = 2 * (N + num_landmarks)
# make the constraint information matrix and vector
Omega = matrix()
Omega.zero(dim, dim)
Omega.value[0][0] = 1.0
Omega.value[1][1] = 1.0
Xi = matrix()
Xi.zero(dim, 1)
Xi.value[0][0] = world_size / 2.0
Xi.value[1][0] = world_size / 2.0
# process the data
for k in range(len(data)):
# n is the index of the robot pose in the matrix/vector
n = k * 2
measurement = data[k][0]
motion = data[k][1]
# integrate the measurements
for i in range(len(measurement)):
# m is the index of the landmark coordinate in the matrix/vector
m = 2 * (N + measurement[i][0])
# update the information maxtrix/vector based on the measurement
for b in range(2):
Omega.value[n+b][n+b] += 1.0 / measurement_noise
Omega.value[m+b][m+b] += 1.0 / measurement_noise
Omega.value[n+b][m+b] += -1.0 / measurement_noise
Omega.value[m+b][n+b] += -1.0 / measurement_noise
Xi.value[n+b][0] += -measurement[i][1+b] / measurement_noise
Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise
# update the information maxtrix/vector based on the robot motion
for b in range(4):
Omega.value[n+b][n+b] += 1.0 / motion_noise
for b in range(2):
Omega.value[n+b ][n+b+2] += -1.0 / motion_noise
Omega.value[n+b+2][n+b ] += -1.0 / motion_noise
Xi.value[n+b ][0] += -motion[b] / motion_noise
Xi.value[n+b+2][0] += motion[b] / motion_noise
# compute best estimate
mu = Omega.inverse() * Xi
# return the result
return mu
def UpdateState(self, measurement):
z = matrix([[measurement[0]], [measurement[1]]])
H = matrix([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0]])
y = z - H * self.x
Ht = H.transpose()
S = H * self.P * Ht + self.R
K = self.P * Ht * S.inverse()
self.x = self.x + K * y
I = matrix([[]])
I.identity(self.n_x)
self.P = (I - K * H) * self.P
def __init__(self, measurement):
self.position = measurement
self.n_x = 3
# self.x [[h], [t], [d]]
self.x = matrix([[]])
self.x.zero(self.n_x, 1)
self.F = matrix([[1, 1, 0], [0, 1, 0], [0, 0, 1]])
self.P = matrix([[1000, 0, 0], [0, 1000, 0], [0, 0, 1000]])
self.R = matrix([[0.01, 0], [0, 0.01]])
self.H = matrix([[1, 1, 0], [0, 0, 1]])
self.number = 0
def generate_samples(samples, p):
mat_x = matrix([[]])
mat_x.zero(len(samples) - p, p)
k = 0
for i in range(0, len(samples) - p):
for j in range(p):
mat_x.value[i][j] = samples[k + j]
k = k + 1
mat_y = matrix([samples[p:len(samples)]])
mat_y = mat_y.transpose()
return mat_x, mat_y
def kalman_xy(x, P, measurement, R, motion = matrix([[0., 0., 0., 0.]]).transpose, Q = matrix([[1., 0.,0.,0.], [0., 1.,0.,0.],[0.,0.,1.,0.],[0.,0.,0.,1.]])):
"""
Parameters:
x: initial state 4-tuple of location and velocity: (x0, x1, x0_dot, x1_dot)
P: initial uncertainty convariance matrix
measurement: observed position
R: measurement noise
motion: external motion added to state vector x
Q: motion noise (same shape as P)
"""
return kalman(x, P, measurement, R, motion, Q, F = matrix([[1., 0., 1., 0.],[0., 1., 0., 1.],[0., 0., 1., 0.],[0., 0., 0., 1.]]), H = matrix([[1., 0., 0., 0.],[0., 1., 0., 0.]]))
def _update(self, measurement, distance_between):
z = matrix([[
atan2(measurement[1] - self.position[1],
measurement[0] - self.position[0])
], [distance_between(measurement, self.position)]])
y = z - self.H * self.x
S = self.H * self.P * self.H.transpose() + self.R
K = self.P * self.H.transpose() * S.inverse()
self.x = self.x + K * y
I = matrix([[]])
I.identity(3)
self.P = (I - K * self.H) * self.P
def saijien(measurement):
K = matrix([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]]) #Coefficient
T = matrix([[0.], [0.], [0.]]) #Constants
S = matrix([[0.], [0.], [0.]]) #Kai
for x, y in measurement:
K += matrix([[x**2, x * y, x], [x * y, y**2, y], [x, y, 1.]])
T -= matrix([[x**3 + x * y**2], [x**2 * y + y**3], [x**2 + y**2]])
S = K.inverse() * T
center = (-S.value[0][0] / 2, -S.value[1][0] / 2)
radius = sqrt(center[0]**2 + center[1]**2 - S.value[2][0])
return center, radius
def update(s, measurement):
H = matrix([[1.,0.,0.,0.], [0.,1.,0.,0.]]) # measurement function: reflect the fact that we observe x and y but not the two velocities
R = matrix([[s.measurement_uncertainty,0.],[0.,s.measurement_uncertainty]]) # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal
I = matrix([[1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,0.,1.]]) # 4d identity matrix
Z = matrix([list(measurement)])
y = Z.transpose() - (H * s.x)
S = H * s.P * H.transpose() + R
K = s.P * H.transpose() * S.inverse()
s.x = s.x + (K * y)
s.P = (I - (K * H)) * s.P
def __init__(self, h, w):
self.pixels = [[screen.DEFAULT[:] for i in range(w)] for i in range(h)]
self.zbuff = [[float("-inf") for i in range(w)] for i in range(h)]
self.height = h
self.width = w
#master matrices
self.tfrm = matrix()
self.stack = [self.tfrm]
self.tfrm.ident()
self.edge = matrix()
self.poly = matrix()
def rotate(self, direction, degree):
if direction == 0:
to_rotate = matrix([[1, 0, 0], [0,
cos(degree), -1 * sin(degree)],
[0, sin(degree), cos(degree)]])
if direction == 1:
to_rotate = matrix([[cos(degree), 0, sin(degree)], [0, 1, 0],
[-1 * sin(degree), 0,
cos(degree)]])
if direction == 2:
to_rotate = matrix([[cos(degree), -1 * sin(degree), 0],
[sin(degree), cos(degree), 0], [0, 0, 1]])
self.forgatas *= to_rotate
def KalmanFilter(measurements):
x = matrix([[measurements[0]], [0]]) # initial state (location and velocity)
P = matrix([[10., 0.], [0., 1000.]]) # initial uncertainty
predict_pos_x, predict_vol_x = filter(x, P, measurements)
predict_pos_x = [item for sublist in predict_pos_x for item in sublist]
predict_pos_x_int = [int(x) for x in predict_pos_x]
print 'predict: '
print predict_pos_x_int
print 'reality: '
print measurements
print 'error:'
print calculateError1D(predict_pos_x_int,measurements)
def initialize_parameters(n_x, n_h, n_y, X):
W1 = matrix([[]])
W1.zero(n_h, n_x)
W1 = fill_random_num(W1)
b1 = matrix([[]])
b1.zero(n_h, X.dimy)
W2 = matrix([[]])
W2.zero(n_y, n_h)
W2 = fill_random_num(W2)
b2 = matrix([[]])
b2.zero(n_y, X.dimy)
parameters = {"W1": W1, "b1": b1, "W2": W2, "b2": b2}
return parameters
def predict(s):
dt = 1
u = matrix([[0.], [0.], [0.], [0.]]) # external motion
F = matrix([[1., 0., dt, 0.], [0., 1., 0., dt], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # next state function: generalize the 2d version to 4d
s.x = (F * s.x) + u
s.P = F * s.P * F.transpose()
# print(s.x)
pos1 = s.x.value[0][0]
pos2 = s.x.value[1][0]
return (pos1, pos2)
def h(self):
# answers: what would be the next measurement according to current state?
newx = matrix([[]])
newx.zero(2,1)
newx.value[0][0] = self.x.value[0][0]
newx.value[1][0] = self.x.value[1][0]
# jacobian
self.H = matrix([
[1., 0., 0., 0., 0.],
[0., 1., 0., 0., 0.]
])
return newx
def predict(s):
dt = 1
u = matrix([[0.], [0.], [0.], [0.]]) # external motion
# F needs to become the Jacobian df/dx
F = matrix([[1., 0., dt, 0.], [0., 1., 0., dt], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # next state function: generalize the 2d version to 4d
Q = matrix([[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.]])
s.x = s.f(s.x, u) #(F * s.x) + u
s.P = F * s.P * F.transpose() + Q
# print(s.x)
pos1 = s.x.value[0][0]
pos2 = s.x.value[1][0]
return (pos1, pos2)
def filter(x, P, measurements, quiet=True):
predict_pos = []
predict_vol = []
for n in range(len(measurements)):
# measurement update
j = H * x
y = matrix([[measurements[n]]])- j #y: 1*1
a = H*P*H.transpose() #a: 1*1
S = a + R #S: 1*1 ##a.inverse()
z = P*H.transpose() #z: 2*1
k = z*S.inverse() #k: 2*1
b = k*y #b: 2*1
x = x + b
P = (I - k*H)*P
# prediction
x = F*x + u
P = F*P*F.transpose()
if not quiet:
print 'x= '
x.show()
print 'P= '
P.show()
predict_pos.append(x.value[0])
predict_vol.append(x.value[1])
return predict_pos, predict_vol
def extended_kalman_filter(z, x, u, P, F_fn, x_fn, H, R):
"""
Applies extended kalman filter on system
z -> measurement
x -> last state
u -> control vector
P -> covariances
F_fn -> Function that returns F matrix for given 'x'
x_fn -> Updates 'x' using the non-linear derivatives
H -> Measurement matrix
R -> Measurement covariance
"""
I = identity_matrix(x.dimx)
# prediction
F = F_fn(x)
x = x_fn(x, u)
P = F * P * F.transpose()
# measurement update
Z = matrix([z])
y = Z.transpose() - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - (K * H)) * P
return x, P
def state_from_measurements(three_measurements):
"""
Estimates state of robot from the last three measurements
Assumes each movement of robot is a "step" and a "turn"
Three measurements constitute two moves, from which turn angle, heading
and step size can be inferred.
"""
x1, y1 = three_measurements[-3]
x2, y2 = three_measurements[-2]
x3, y3 = three_measurements[-1]
# Last two position vectors from measurements
vec_1 = [x2 - x1, y2 - y1]
vec_2 = [x3 - x2, y3 - y2]
# Find last turning angle using dot product
dot = sum(v1*v2 for v1,v2 in zip(vec_1, vec_2))
mag_v1 = sqrt(sum(v**2 for v in vec_1))
mag_v2 = sqrt(sum(v**2 for v in vec_2))
v0 = mag_v2
w0 = acos(dot/(mag_v1*mag_v2))
if dot < 0:
w0 = pi-w0
theta0 = atan2(vec_2[1], vec_2[0]) + w0
x0 = x3 + v0*cos(theta0 + w0)
y0 = y3 + v0*sin(theta0 + w0)
return matrix([[x3], [y3], [theta0], [v0], [w0]])
def kalman(x, P, measurement, R, motion, Q, F, H):
'''
Parameters:
x: initial state
P: initial uncertainty convariance matrix
measurement: observed position (same shape as H*x)
R: measurement noise (same shape as H)
motion: external motion added to state vector x
Q: motion noise (same shape as P)
F: next state function: x_prime = F*x
H: measurement function: position = H*x
Return: the updated and predicted new values for (x, P)
See also http://en.wikipedia.org/wiki/Kalman_filter
This version of kalman can be applied to many different situations by
appropriately defining F and H
'''
# UPDATE x, P based on measurement m
# distance between measured and current position-belief
y = matrix([measurement])
y = y.transpose - H * x
S = H * P * H.transpose + R # residual convariance
K = P * H.transpose * S.inverse # Kalman gain
x = x + K*y
size = F.dimx
I = matrix.identity(size) # identity matrix
P = (I - K*H)*P
# PREDICT x, P based on motion
x = F*x + motion
P = F*P*F.transpose + Q
return x, P
def testcp(dim,verbose=False) : # characteristic polynomial test
X = matrix.Identity(dim,x);
M = matrix(dim,dim,
tuple(xrational(random()+random()*1j) for i in range(dim*dim)))
cp = (M-X).det;
try :
if cp.denominator != 1 :
print('Charpoly not a polynomial? Denominator = %s'
%(cp.denominator.mapcoeffs(complex)));
verbose=True;
cp = cp.numerator;
except :
pass; # was already polynomial
Md = M.det;
if verbose or cp(0) != Md or cp(M) :
print(M.mapped(complex));
print(cp.mapcoeffs(complex));
if cp(0) != Md : # check that characteristic polynomial at 0 is determinant
print('det %s != charpoly(0) %s'%(float(Md),float(cp(0))));
cpM = cp(M);
if cp(M) : # check that matrix satisfies its characteristic polynomial
print(M,cpM);
MT = M.T
cpMT = cp(MT);
if cpMT : # check that transposed matrix satisfies characteristic polynomial
print(MT,cpMT);
def g(self):
distancediff = self.x.value[2][0]
anglediff = self.x.value[3][0]
theta = self.x.value[4][0] + anglediff
x = self.x.value[0][0] + cos(theta) * distancediff
y = self.x.value[1][0] + sin(theta) * distancediff
# jacobian
self.G = matrix([
[1., 0., cos(self.x.value[4][0]), 0., -1 * sin(self.x.value[4][0]) * distancediff],
[0., 1., sin(self.x.value[4][0]), 0., cos(self.x.value[4][0]) * distancediff],
[0., 0., 1., 0., 0.],
[0., 0., 0., 1., 0.],
[0., 0., 0., 1., 1.]
])
moved_angle = atan2(y - self.x.value[1][0], x - self.x.value[0][0])
new_angle_diff = (moved_angle - self.x.value[4][0]) % (pi * 2)
new_distance_diff = distance_between([x,y], [self.x.value[0][0],self.x.value[1][0]])
self.x.value[0][0] = x
self.x.value[1][0] = y
self.x.value[2][0] = new_distance_diff
self.x.value[3][0] = new_angle_diff
self.x.value[4][0] = theta
return self.x
def estimate_next_pos(measurement, OTHER = None):
"""Estimate the next (x, y) position of the wandering Traxbot
based on noisy (x, y) measurements."""
if OTHER is None:
# Setup Kalman Filter
[u, P, H, R] = setup_kalman_filter()
# OTHER = {'x': x, 'P': P, 'u': u, 'matrices':[H, R]}
x = matrix([[measurement[0]], [measurement[1]], [0], [0], [0]])
OTHER = {'z_list': deque([]), 'x': x,
'P': P, 'u': u, 'matrices': [H, R], 'step': 1
# 'zx': [measurement[0]]
}
OTHER['z_list'].append(np.array(measurement))
# return measurement, OTHER
# elif OTHER['step'] == 1:
# # Use first three measurements to seed the filter
# OTHER['step'] = 2
# OTHER['z_list'].append(np.array(measurement))
# # OTHER['zx'].append(measurement[0])
# # OTHER['x_list'].append(measurement)
# return measurement, OTHER
# elif OTHER['step'] == 2:
# OTHER['step'] = 3
# # Get last 3 measurements
# OTHER['z_list'].append(np.array(measurement))
# # OTHER['zx'].append(measurement[0])
# # Get initial estimate of state from the three measurements
# OTHER['x'] = state_from_measurements(OTHER['z_list'])
#
# # Initialization complete
# OTHER['step'] = -1
#
# # Use last 20 measurements only
# num_z = 1000
# # OTHER['x_list'] = deque(maxlen=num_z)
# # OTHER['z_list'] = deque(maxlen=num_z+1)
#
# # Predict next position of robot using the dynamics and current state
# next_state = robot_x_fn(OTHER['x'])
# # OTHER['x_list'].append(next_state)
# return (next_state.value[0][0], next_state.value[1][0]), OTHER
OTHER['z_list'].append(np.array(measurement))
x, P = extended_kalman_filter(measurement, OTHER['x'], OTHER['u'],
OTHER['P'], robot_F_fn, robot_x_fn, *OTHER['matrices'])
# OTHER['x_list'].append(x)
OTHER['x'] = x
OTHER['P'] = P
# print('Trace of P : '+str(P.trace()))
# Predict next position of robot
next_state = robot_x_fn(x)
est_xy = (next_state.value[0][0], next_state.value[1][0])
# You must return xy_estimate (x, y), and OTHER (even if it is None)
# in this order for grading purposes.
# xy_estimate = (3.2, 9.1)
# return z, OTHER
return est_xy, OTHER
def Vandermonde(xs,k=0) : """Given a list of "numbers" and an optional degree (default: length of list), return a Vandermonde matrix with rows corresponding to the "numbers" and columns corresponding to powers from 0 to degree-1""" n = len(xs); k = k or n; return matrix(n,k,[x**i for i in range(k) for x in xs]);
def calculate(measurements, initial_xy):
dt = 0.1
x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity)
u = matrix([[0.], [0.], [0.], [0.]]) # external motion
### fill this in: ###
#P = # initial uncertainty
P = matrix([[0.,0.,0.,0.],[0.,0.,0.,0.],[0.,0.,1000.,0.],[0.,0.,0.,1000.]])
#F = # next state function
F = matrix([[1.,0.,dt,0.],[0.,1.,0.,dt],[0.,0.,1.,0.],[0.,0.,0.,1.]])
#H = # measurement function
H = matrix([[1.,0.,0.,0.],[0.,1.,0.,0.]])
#R = # measurement uncertainty
R = matrix([[0.1,0.],[0.,0.1]])
#I = # identity matrix
I = matrix([[1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,0.,1.]])
def filter(x, P):
for n in range(len(measurements)):
# prediction
x = (F * x) + u
P = F * P * F.transpose()
# measurement update
Z = matrix([measurements[n]])
y = Z.transpose() - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - (K * H)) * P
return x, P
return filter(x, P)
def __init__(self, x, u, P, R):
self.x = x
self.u = u
self.P = P
self.R = R
# Identity matrix should have the same dimensionality as state space
self.I = matrix([[]])
self.I.identity(x.dimx)
def demo_kalman_xy2():
observed_x = (3.2618187593136714, 4.229518090705041, 4.970144154070997, 5.4584758171066685, 5.677883530716649, 5.620895628364498, 5.289452764715364, 4.694841828968918, 3.857311583384793, 2.8053831159695455)
observed_y = (5.248776670511476,6.394882252046046,7.699287493626686,9.117572439213015,10.601439096028392,12.10035616477686,13.563279822435065,14.940391958424991,16.18479667061946,17.25411724904363)
x = matrix([[0.], [0.], [0.], [0.]])
P = matrix([[1000., 0.,0.,0.], [0., 1000.,0.,0.],[0.,0.,1000.,0.],[0.,0.,0.,1000.]]) # initial uncertainty
plt.plot(observed_x, observed_y, 'ro')
result = []
R = 0.01**2
for meas in zip(observed_x, observed_y):
x, P = kalman_xy(x, P, meas, R)
result.append((x[:2]).tolist())
kalman_x, kalman_y = zip(*result)
plt.plot(kalman_x, kalman_y, 'g-')
plt.show()
def estimate_next_pos(measurement, OTHER = None):
xy_estimate = measurement
if OTHER == None or not OTHER.has_key("initialized"):
xy_estimate = measurement
if OTHER == None:
OTHER = dict()
if OTHER.has_key("measurement"):
OTHER["initialized"] = True
OTHER["angle"] = atan2(measurement[1] - OTHER["measurement"][1], measurement[0] - OTHER["measurement"][0])
OTHER["measurement"] = measurement
elif not OTHER.has_key("kalman"):
moved_angle = atan2(measurement[1] - OTHER["measurement"][1], measurement[0] - OTHER["measurement"][0])
angle_diff = moved_angle - OTHER["angle"]
distance_diff = distance_between(measurement, OTHER["measurement"])
x = matrix([[measurement[0]], [measurement[1]], [distance_diff], [angle_diff], [moved_angle]])
u = matrix([[0.], [0.], [0.], [0.], [0.]])
# initial uncerties
P = matrix([
[0.05, 0., 0., 0., 0.],
[0., 0.05, 0., 0., 0.],
[0., 0., 0.05, 0., 0.],
[0., 0., 0., 0.05, 0.],
[0., 0., 0., 0., 0.05]
])
# measurement uncertainty
R = matrix([
[0.1, 0.],
[0., 0.1],
])
OTHER["kalman"] = RobotFilter(x, u, P, R)
OTHER["kalman"].measure([measurement[0], measurement[1]])
OTHER["kalman"].predict()
else:
OTHER["kalman"].measure([measurement[0], measurement[1]])
estimate = OTHER["kalman"].predict()
xy_estimate = (estimate.value[0][0], estimate.value[1][0])
return xy_estimate, OTHER
def F_matrix(state):
h = state.value[2][0]
t = state.value[3][0]
d = state.value[4][0]
return matrix([[1., 0., -d * sin(h + t), -d * sin(h + t), cos(h + t)],
[0., 1., d * cos(h + t), d * cos(h + t), sin(h + t)],
[0., 0., 1., 1., 0.],
[0., 0., 0., 1., 0.],
[0., 0., 0., 0., 1.]])
def next_move(hunter_position, hunter_heading, target_measurement, max_distance, OTHER = None):
# This function will be called after each time the target moves.
if not OTHER: # first time calling this function, set up my OTHER variables.
measurements = []
P = matrix([[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1000.0, 0.0], [0.0, 0.0, 0.0, 1000.0]])
old_target = (0.0, 0.0)
theta = 0.0
else: # not the first time, update my history
measurements = OTHER[0]
P = OTHER[1]
old_target = OTHER[2]
theta = OTHER[3]
print "NOISY MEASUREMENT: ", target_measurement
x = matrix([[target_measurement[0]], [target_measurement[1]], [0.], [0.]])
new_x, P = filter(x, P, measurements[-25:])
xprime = new_x.value[0][0]
yprime = new_x.value[1][0]
fixed_target = (xprime, yprime)
print "FIXED MEASUREMENT: ", fixed_target
delta_y = fixed_target[1] - old_target[1]
delta_x = fixed_target[0] - old_target[0]
beta = atan2(delta_y, delta_x)
db = distance_between(old_target, fixed_target)
turn_angle = beta - theta
heading = beta + turn_angle
future_x = fixed_target[0] + db * cos(heading)
future_y = fixed_target[1] + db * sin(heading)
future_target = (future_x, future_y)
heading_to_target = get_heading(hunter_position, future_target)
heading_difference = heading_to_target - hunter_heading
turning = heading_difference # turn towards the target
measurements.append(fixed_target)
distance = distance_between(hunter_position, future_target)
# distance = max_distance
OTHER = [measurements, P, fixed_target, beta]
return turning, distance, OTHER
def measure(self, measurement):
# a posteriori
Z = matrix([measurement])
y = Z.transpose() - self.h()
S = self.H * self.P * self.H.transpose() + self.R
K = self.P * self.H.transpose() * S.inverse()
self.x = self.x + (K * y)
self.P = (self.I - (K * self.H)) * self.P
return self.x
def f(state):
x = state.value[0][0]
y = state.value[1][0]
h = state.value[2][0]
t = state.value[3][0]
d = state.value[4][0]
h += t
x += d * cos(h)
y += d * sin(h)
return matrix([[x], [y], [h], [t], [d]])
def homology_generators(d2, d1, as_matrix_with_column_vectors = False):
"""
As for base_change_chain_complex, consider the following chain complex over
a field:
C_2 ------> C_1 ------> C_0
d_2 d_1
homology_generators will return a list of a representatives in C_1 for
each generator of H_1. If as_matrix_with_column_vectors is true, it will
return a matrix where each column vector is a representative in C_1.
>>> from fractions import Fraction
>>> Fraction.one=staticmethod(lambda : Fraction(1,1))
>>> Fraction.zero=staticmethod(lambda : Fraction(0,1))
>>> d2 = matrix([[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]],
... Fraction)
>>> d1 = matrix([[0,0,0,0,0],
... [0,0,0,0,0],
... [0,0,0,0,0],
... [0,0,0,0,0],
... [0,0,0,0,1],
... [0,0,0,0,0]],
... Fraction)
>>> homology_generators(d2,d1)
[[Fraction(0, 1), Fraction(0, 1), Fraction(0, 1), Fraction(1, 1), Fraction(0, 1)]]
>>> d2, d1 = generate_test_example_homology_generators(Fraction,5,10,15,3,2)
>>> H = homology_generators(d2, d1)
>>> len(H)
2
>>> for h in H:
... for i in d1 * h:
... if not i == Fraction(0, 1):
... raise Exception
"""
gens=[]
n_d2, n_d1, base, base_inv = base_change_chain_complex(d2, d1)
for i in range(n_d1.no_columns()):
if (not non_zero_column(n_d1, i)) and (not non_zero_row(n_d2, i)):
gens.append([r[i] for r in base.values])
if as_matrix_with_column_vectors:
return matrix(gens, d1.the_type).transpose()
else:
return gens
def kalman_filter(x, P, measurement):
# measurement update
z = matrix([[measurement]])
# z.identity(1, measurement[0])
# print z
Hx = H * x
y = z - Hx
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
Ky = K * y
x = x + Ky
KH = K * H
# print KH.dimx, KH.dimy
# print KH.dimx
one = matrix([[1]])
# one.identity(KH.dimx,1)
# print one
P = (one - KH) * P
# print "After measurement " + str(n)
# print "x:"
# x.show()
# print "P:"
# P.show()
# prediction
x = F * x + u
P = F * P * F.transpose()
# print "After prediction " + str(n)
# print "x:"
# x.show()
# print "P:"
# P.show()
# print x,P
return x, P
def kalman_filter2(x2, P2, measurement2):
# measurement update
z2 = matrix([[measurement2]])
# z.identity(1, measurement[0])
# print z2
Hx = H2 * x2
y = z2 - Hx
S = H2 * P2 * H2.transpose() + R2
K = P2 * H2.transpose() * S.inverse()
Ky = K * y
x2 = x2 + Ky
KH = K * H2
# print KH.dimx, KH.dimy
# print KH.dimx
one = matrix([[1]])
# one.identity(KH.dimx,1)
# print one
P = (one - KH) * P2
# print "After measurement " + str(n)
# print "x:"
# x.show()
# print "P:"
# P.show()
# prediction
x = F2 * x2 + u2
P2 = F2 * P2 * F2.transpose()
# print "After prediction " + str(n)
# print "x:"
# x.show()
# print "P:"
# P.show()
print x2, P2
return x2, P2
def content( self ): B = matrix( self.order + 2 ) for i in range( self.order + 2 ): for j in range( self.order + 2 ): if i == j: B.set_value( i, j, 0 ) elif i == 0 or j == 0: B.set_value( i, j, 1 ) else: B.set_value( i, j, ( self.vertices[ i - 1 ] - self.vertices[ j - 1 ] ).length_squared() ) from math import factorial, sqrt coefficient = ( (-1) ** ( self.order + 1 ) ) / float( ( 2 ** self.order ) * ( factorial( self.order ) ** 2 ) ) return sqrt( coefficient * determinant( B ) )
def main():
initEKF = None #Initial Extended Kalman Filter Generated from the training_data.txt
initEKF = 'initialKF.txt'#file we are saving our initial Extended Kalman Filter
try:
initEKF = open('initialKF.txt')
initEKF = matrix(initEKF.read())
except:
print 'No initial EKF found: generating new one \n'
#initEKF = generateInitialEKF(initEKF)
testFiles = ['test05.txt']
#testFiles = ['test01.txt','test02.txt','test03.txt','test04.txt','test05.txt','test06.txt','test07.txt','test08.txt','test09.txt','test10.txt']
for testFile in testFiles:
data = readData(testFile)
