count = [] # varible to hold the last position to for plotting reasons last = [my_x[0], my_y[0]] pos = last outliers = 0 x_good = True y_good = True for i in xrange(len(t)): # if not outlier out it into kalman filter x_good = True y_good = True if (md_filter.update([my_x[i], my_y[i]])): outliers = outliers + 1 x_good = poly_x.update(my_x[i]) y_good = poly_y.update(my_y[i]) if x_good and y_good: out_x.append(my_x[i]) out_y.append(my_y[i]) count.append(i) temp_x = particle_x.update(0, my_x[ii]) temp_y = particle_y.update(0, my_y[ii]) new_x.append(temp_x) new_y.append(temp_y) pos = [temp_x, temp_y] else: if len(out_x) == 0:
kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, pos) md_filter = md_filter.md_filter(3, [1.6, 1.5], 70, [-30, 30]) out_x = [] out_y = [] out_z = [] new_x = [] new_y = [] new_z = [] last = pos outliers = 0 for i in xrange(len(t)): if (md_filter.update([x[i], y[i], z[i]])): out_x.append(x[i]) out_y.append(y[i]) out_z.append(z[i]) Z = np.matrix([[0], [0], [0], [x[i]], [y[i]], [z[i]]]) U = np.matrix([[0]]) kalman.move(U, Z) pos = kalman.getState() new_x.append(pos[3]) new_y.append(pos[4]) new_z.append(pos[5]) last = pos
B = np.matrix([0]) C = np.eye(pos.shape[0]) Q = np.eye(pos.shape[0]) * q R = np.eye(pos.shape[0]) * r P = np.eye(pos.shape[0]) U = np.matrix([[0]]) Z = np.matrix([[0], [0], [0], [0], [0], [0]]) kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, pos) md_filter = md_filter.md_filter(3, [1.6, 1.5], 20, [-180, 180]) last = pos outliers = 0 for i in xrange(len(t)): if (md_filter.update([roll[i], pitch[i], yaw[i]])): out_roll.append(roll[i]) out_pitch.append(pitch[i]) out_yaw.append(yaw[i]) Z = np.matrix([[0], [0], [0], [roll[i]], [pitch[i]], [yaw[i]]]) U = np.matrix([[0]]) kalman.move(U, Z) pos = kalman.getState() new_roll.append(pos[3]) new_pitch.append(pos[4]) new_yaw.append(pos[5]) last = pos