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