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)