def testSimpleFilterDecayThenKalman(): with open('input.csv', 'rb') as inputFile: with open('output.csv', 'w') as outputFile: outputFieldnames = [ 'time', 'target1_x', 'target1_y', 'target1_z', \ 'target2_x', 'target2_y', 'target2_z', \ 'lasers_yaw', \ 'local_vel_x', 'local_vel_y', 'local_vel_z', 'local_vel_yaw', \ 'decay1_x', 'decay1_y', 'decay1_z', \ 'decay2_x', 'decay2_y', 'decay2_z', \ 'decay_yaw', \ 'filtered_x', 'filtered_y', 'filtered_z', 'filtered_yaw' ] fieldnames = ['time', 'setpoint_x', 'setpoint_y', 'setpoint_z', 'setpoint_yaw', \ 'local_x', 'local_y', 'local_z', 'local_roll', 'local_pitch', 'local_yaw', \ 'lasers_x', 'lasers_y', 'lasers_z', 'lasers_yaw', \ 'raw_x_1', 'raw_x_2', 'raw_y_1', 'raw_y_2','raw_z_1', 'raw_z_2', \ 'local_vel_x', 'local_vel_y', 'local_vel_z', 'local_vel_yaw', \ 'accel_lin_x', 'accel_lin_y', 'accel_lin_z', \ 'orientation_x', 'orientation_y', 'orientation_z', 'orientation_w', \ 'filtered_x', 'filtered_y', 'filtered_z', 'filtered_yaw', \ 'accel_no_gravity_x', 'accel_no_gravity_y', 'accel_no_gravity_z', \ 'Xk_x', 'Xk_y', 'Xk_z', 'Xk_yaw', \ 'K_x', 'K_y', 'K_z', 'K_yaw', \ 'Xkp_x', 'Xkp_y', 'Xkp_z', 'Xkp_yaw', \ 'target1_x', 'target1_y', 'target1_z', \ 'target2_x', 'target2_y', 'target2_z', \ 'isArmed?', 'information_utilisateur'] data_reader = csv.DictReader(inputFile, fieldnames=fieldnames, delimiter=';', quotechar='|', quoting=csv.QUOTE_MINIMAL) data_writer = csv.writer(outputFile, outputFile, lineterminator='\n') FilterX = simple_filter() FilterY = simple_filter() FilterZ = simple_filter() FilterYaw = simple_filter() LPX_velocity = simple_decay_filter(0.93) LPY_velocity = simple_decay_filter(0.93) LPZ_velocity = simple_decay_filter(0.93) LPX193 = simple_decay_filter(0.93) LPY193 = simple_decay_filter(0.93) LPZ193 = simple_decay_filter(0.93) LPX293 = simple_decay_filter(0.93) LPY293 = simple_decay_filter(0.93) LPZ293 = simple_decay_filter(0.93) LPYaw93 = simple_decay_filter(0.93) lasttime = 0.0 count = 0 for row in data_reader: if (count == 2): data_writer.writerow(outputFieldnames) if (count > 6): # target = (X1, X2, Y1, Y2, Z1, Z2, Yaw1, Yaw2) in m/s and rad # linearSpeed = (Vx, Vy, Vz) in m/s # LinearAcceleration = (Ax, Ay, Az) in m/s/s # AngularSpeed = yawRate in rad/s # dt in seconds # Kalman.next(target, linearSpeed, linearAcceleration, angularSpeed, dt) Measures = [[float(row['target1_x']), float(row['target2_x'])], \ [float(row['target1_y']), float(row['target2_y'])], \ [float(row['target1_z']), float(row['target2_z'])], \ float(row['lasers_yaw'])] linearSpeed = [ float(row['local_vel_x']), \ float(row['local_vel_y']), \ float(row['local_vel_z'])] angularSpeed = [float(row['local_vel_yaw'])] dt = float(row['time']) - lasttime LPX_1 = LPX193.next(Measures[0][0]) LPY_1 = LPY193.next(Measures[1][0]) LPZ_1 = LPZ193.next(Measures[2][0]) LPX_2 = LPX293.next(Measures[0][1]) LPY_2 = LPY293.next(Measures[1][1]) LPZ_2 = LPZ293.next(Measures[2][1]) LPYaw = LPYaw93.next(Measures[3]) velX_filtered = LPX_velocity.next(linearSpeed[0]) velY_filtered = LPY_velocity.next(linearSpeed[1]) velZ_filtered = LPZ_velocity.next(linearSpeed[2]) linearSpeed = [velX_filtered, velY_filtered, velZ_filtered] X = FilterX.next([LPX_1, LPX_2], dt, linearSpeed[0]) Y = FilterY.next([LPY_1, LPY_2], dt, linearSpeed[1]) Z = FilterZ.next([LPZ_1, LPZ_2], dt, linearSpeed[2]) Yaw = FilterYaw.next(LPYaw, dt, angularSpeed[0]) lasttime = float(row['time']) data_writer.writerow([row['time'], row['target1_x'], row['target1_y'], row['target1_z'], \ row['target2_x'], row['target2_y'], row['target2_z'], \ row['lasers_yaw'], \ row['local_vel_x'], row['local_vel_y'], row['local_vel_z'], row['local_vel_yaw'], \ str(LPX_1), str(LPY_1), str(LPZ_1), \ str(LPX_2), str(LPY_2), str(LPZ_2), \ str(LPYaw), \ str(X), str(Y), str(Z), str(Yaw) \ ]) count = count + 1
def testSimpleFilterDecayThenKalman(): with open('input.csv', 'rb') as inputFile: with open('output.csv', 'w') as outputFile: outputFieldnames = [ 'time', 'target1_x', 'target1_y', 'target1_z', \ 'target2_x', 'target2_y', 'target2_z', \ 'lasers_yaw', \ 'local_vel_x', 'local_vel_y', 'local_vel_z', 'local_vel_yaw', \ 'decay1_x', 'decay1_y', 'decay1_z', \ 'decay2_x', 'decay2_y', 'decay2_z', \ 'decay_yaw', \ 'filtered_x', 'filtered_y', 'filtered_z', 'filtered_yaw' ] fieldnames = ['time', 'setpoint_x', 'setpoint_y', 'setpoint_z', 'setpoint_yaw', \ 'local_x', 'local_y', 'local_z', 'local_roll', 'local_pitch', 'local_yaw', \ 'lasers_x', 'lasers_y', 'lasers_z', 'lasers_yaw', \ 'raw_x_1', 'raw_x_2', 'raw_y_1', 'raw_y_2','raw_z_1', 'raw_z_2', \ 'local_vel_x', 'local_vel_y', 'local_vel_z', 'local_vel_yaw', \ 'accel_lin_x', 'accel_lin_y', 'accel_lin_z', \ 'orientation_x', 'orientation_y', 'orientation_z', 'orientation_w', \ 'filtered_x', 'filtered_y', 'filtered_z', 'filtered_yaw', \ 'accel_no_gravity_x', 'accel_no_gravity_y', 'accel_no_gravity_z', \ 'Xk_x', 'Xk_y', 'Xk_z', 'Xk_yaw', \ 'K_x', 'K_y', 'K_z', 'K_yaw', \ 'Xkp_x', 'Xkp_y', 'Xkp_z', 'Xkp_yaw', \ 'target1_x', 'target1_y', 'target1_z', \ 'target2_x', 'target2_y', 'target2_z', \ 'isArmed?', 'information_utilisateur'] data_reader = csv.DictReader(inputFile, fieldnames=fieldnames, delimiter=';', quotechar='|', quoting=csv.QUOTE_MINIMAL) data_writer = csv.writer(outputFile,outputFile, lineterminator='\n') FilterX = simple_filter() FilterY = simple_filter() FilterZ = simple_filter() FilterYaw = simple_filter() LPX_velocity = simple_decay_filter(0.93) LPY_velocity = simple_decay_filter(0.93) LPZ_velocity = simple_decay_filter(0.93) LPX193 = simple_decay_filter(0.93) LPY193 = simple_decay_filter(0.93) LPZ193 = simple_decay_filter(0.93) LPX293 = simple_decay_filter(0.93) LPY293 = simple_decay_filter(0.93) LPZ293 = simple_decay_filter(0.93) LPYaw93 = simple_decay_filter(0.93) lasttime = 0.0 count = 0 for row in data_reader : if(count == 2): data_writer.writerow(outputFieldnames) if(count > 6): # target = (X1, X2, Y1, Y2, Z1, Z2, Yaw1, Yaw2) in m/s and rad # linearSpeed = (Vx, Vy, Vz) in m/s # LinearAcceleration = (Ax, Ay, Az) in m/s/s # AngularSpeed = yawRate in rad/s # dt in seconds # Kalman.next(target, linearSpeed, linearAcceleration, angularSpeed, dt) Measures = [[float(row['target1_x']), float(row['target2_x'])], \ [float(row['target1_y']), float(row['target2_y'])], \ [float(row['target1_z']), float(row['target2_z'])], \ float(row['lasers_yaw'])] linearSpeed = [ float(row['local_vel_x']), \ float(row['local_vel_y']), \ float(row['local_vel_z'])] angularSpeed = [float(row['local_vel_yaw'])] dt = float(row['time']) - lasttime LPX_1 = LPX193.next(Measures[0][0]) LPY_1 = LPY193.next(Measures[1][0]) LPZ_1 = LPZ193.next(Measures[2][0]) LPX_2 = LPX293.next(Measures[0][1]) LPY_2 = LPY293.next(Measures[1][1]) LPZ_2 = LPZ293.next(Measures[2][1]) LPYaw = LPYaw93.next(Measures[3]) velX_filtered = LPX_velocity.next(linearSpeed[0]) velY_filtered = LPY_velocity.next(linearSpeed[1]) velZ_filtered = LPZ_velocity.next(linearSpeed[2]) linearSpeed = [velX_filtered, velY_filtered, velZ_filtered] X = FilterX.next([LPX_1, LPX_2], dt, linearSpeed[0]) Y = FilterY.next([LPY_1, LPY_2], dt, linearSpeed[1]) Z = FilterZ.next([LPZ_1, LPZ_2], dt, linearSpeed[2]) Yaw = FilterYaw.next(LPYaw, dt, angularSpeed[0]) lasttime = float(row['time']) data_writer.writerow([row['time'], row['target1_x'], row['target1_y'], row['target1_z'], \ row['target2_x'], row['target2_y'], row['target2_z'], \ row['lasers_yaw'], \ row['local_vel_x'], row['local_vel_y'], row['local_vel_z'], row['local_vel_yaw'], \ str(LPX_1), str(LPY_1), str(LPZ_1), \ str(LPX_2), str(LPY_2), str(LPZ_2), \ str(LPYaw), \ str(X), str(Y), str(Z), str(Yaw) \ ]) count = count +1
def testSimpleFilterKalmanThenDecay(): with open('input.csv', 'rb') as inputFile: with open('output.csv', 'w') as outputFile: outputFieldnames = [ 'time', 'target1_x', 'target1_y', 'target1_z', \ 'target2_x', 'target2_y', 'target2_z', \ 'lasers_yaw', \ 'local_vel_x', 'local_vel_y', 'local_vel_z', 'local_vel_yaw', \ 'filtered_x', 'filtered_y', 'filtered_z', 'filtered_yaw', \ 'low97_x', 'low97_y', 'low97_z', 'low97_yaw', \ 'low93_x', 'low93_y', 'low93_z', 'low93_yaw', \ 'low89_x', 'low89_y', 'low89_z', 'low89_yaw' ] fieldnames = ['time', 'setpoint_x', 'setpoint_y', 'setpoint_z', 'setpoint_yaw', \ 'local_x', 'local_y', 'local_z', 'local_roll', 'local_pitch', 'local_yaw', \ 'lasers_x', 'lasers_y', 'lasers_z', 'lasers_yaw', \ 'raw_x_1', 'raw_x_2', 'raw_y_1', 'raw_y_2','raw_z_1', 'raw_z_2', \ 'local_vel_x', 'local_vel_y', 'local_vel_z', 'local_vel_yaw', \ 'accel_lin_x', 'accel_lin_y', 'accel_lin_z', \ 'orientation_x', 'orientation_y', 'orientation_z', 'orientation_w', \ 'filtered_x', 'filtered_y', 'filtered_z', 'filtered_yaw', \ 'accel_no_gravity_x', 'accel_no_gravity_y', 'accel_no_gravity_z', \ 'Xk_x', 'Xk_y', 'Xk_z', 'Xk_yaw', \ 'K_x', 'K_y', 'K_z', 'K_yaw', \ 'Xkp_x', 'Xkp_y', 'Xkp_z', 'Xkp_yaw', \ 'target1_x', 'target1_y', 'target1_z', \ 'target2_x', 'target2_y', 'target2_z', \ 'isArmed?', 'information_utilisateur'] data_reader = csv.DictReader(inputFile, fieldnames=fieldnames, delimiter=';', quotechar='|', quoting=csv.QUOTE_MINIMAL) data_writer = csv.writer(outputFile, outputFile, lineterminator='\n') FilterX = simple_filter() FilterY = simple_filter() FilterZ = simple_filter() FilterYaw = simple_filter() LPX97 = simple_decay_filter(0.97) LPY97 = simple_decay_filter(0.97) LPZ97 = simple_decay_filter(0.97) LPYaw97 = simple_decay_filter(0.97) LPX93 = simple_decay_filter(0.93) LPY93 = simple_decay_filter(0.93) LPZ93 = simple_decay_filter(0.93) LPYaw93 = simple_decay_filter(0.93) LPX89 = simple_decay_filter(0.89) LPY89 = simple_decay_filter(0.89) LPZ89 = simple_decay_filter(0.89) LPYaw89 = simple_decay_filter(0.89) lasttime = 0.0 count = 0 for row in data_reader: if (count == 2): data_writer.writerow(outputFieldnames) if (count > 6): # target = (X1, X2, Y1, Y2, Z1, Z2, Yaw1, Yaw2) in m/s and rad # linearSpeed = (Vx, Vy, Vz) in m/s # LinearAcceleration = (Ax, Ay, Az) in m/s/s # AngularSpeed = yawRate in rad/s # dt in seconds # Kalman.next(target, linearSpeed, linearAcceleration, angularSpeed, dt) Measures = [[float(row['target1_x']), float(row['target2_x'])], \ [float(row['target1_y']), float(row['target2_y'])], \ [float(row['target1_z']), float(row['target2_z'])], \ float(row['lasers_yaw'])] linearSpeed = [ float(row['local_vel_x']), \ float(row['local_vel_y']), \ float(row['local_vel_z'])] angularSpeed = [float(row['local_vel_yaw'])] dt = float(row['time']) - lasttime X = FilterX.next(Measures[0], dt, linearSpeed[0]) Y = FilterY.next(Measures[1], dt, linearSpeed[1]) Z = FilterZ.next(Measures[2], dt, linearSpeed[2]) Yaw = FilterYaw.next(Measures[3], dt, angularSpeed[0]) X_low_97 = LPX97.next(X) Y_low_97 = LPY97.next(Y) Z_low_97 = LPZ97.next(Z) Yaw_low_97 = LPYaw97.next(Yaw) X_low_93 = LPX93.next(X) Y_low_93 = LPY93.next(Y) Z_low_93 = LPZ93.next(Z) Yaw_low_93 = LPYaw93.next(Yaw) X_low_89 = LPX89.next(X) Y_low_89 = LPY89.next(Y) Z_low_89 = LPZ89.next(Z) Yaw_low_89 = LPYaw89.next(Yaw) lasttime = float(row['time']) data_writer.writerow([row['time'], row['target1_x'], row['target1_y'], row['target1_z'], \ row['target2_x'], row['target2_y'], row['target2_z'], \ row['lasers_yaw'], \ row['local_vel_x'], row['local_vel_y'], row['local_vel_z'], row['local_vel_yaw'], \ str(X), str(Y), str(Z), str(Yaw),\ str(X_low_97), str(Y_low_97), str(Z_low_97), str(Yaw_low_97), \ str(X_low_93), str(Y_low_93), str(Z_low_93), str(Yaw_low_93), \ str(X_low_89), str(Y_low_89), str(Z_low_89), str(Yaw_low_89) \ ]) count = count + 1
def testSimpleFilterKalmanThenDecay(): with open('input.csv', 'rb') as inputFile: with open('output.csv', 'w') as outputFile: outputFieldnames = [ 'time', 'target1_x', 'target1_y', 'target1_z', \ 'target2_x', 'target2_y', 'target2_z', \ 'lasers_yaw', \ 'local_vel_x', 'local_vel_y', 'local_vel_z', 'local_vel_yaw', \ 'filtered_x', 'filtered_y', 'filtered_z', 'filtered_yaw', \ 'low97_x', 'low97_y', 'low97_z', 'low97_yaw', \ 'low93_x', 'low93_y', 'low93_z', 'low93_yaw', \ 'low89_x', 'low89_y', 'low89_z', 'low89_yaw' ] fieldnames = ['time', 'setpoint_x', 'setpoint_y', 'setpoint_z', 'setpoint_yaw', \ 'local_x', 'local_y', 'local_z', 'local_roll', 'local_pitch', 'local_yaw', \ 'lasers_x', 'lasers_y', 'lasers_z', 'lasers_yaw', \ 'raw_x_1', 'raw_x_2', 'raw_y_1', 'raw_y_2','raw_z_1', 'raw_z_2', \ 'local_vel_x', 'local_vel_y', 'local_vel_z', 'local_vel_yaw', \ 'accel_lin_x', 'accel_lin_y', 'accel_lin_z', \ 'orientation_x', 'orientation_y', 'orientation_z', 'orientation_w', \ 'filtered_x', 'filtered_y', 'filtered_z', 'filtered_yaw', \ 'accel_no_gravity_x', 'accel_no_gravity_y', 'accel_no_gravity_z', \ 'Xk_x', 'Xk_y', 'Xk_z', 'Xk_yaw', \ 'K_x', 'K_y', 'K_z', 'K_yaw', \ 'Xkp_x', 'Xkp_y', 'Xkp_z', 'Xkp_yaw', \ 'target1_x', 'target1_y', 'target1_z', \ 'target2_x', 'target2_y', 'target2_z', \ 'isArmed?', 'information_utilisateur'] data_reader = csv.DictReader(inputFile, fieldnames=fieldnames, delimiter=';', quotechar='|', quoting=csv.QUOTE_MINIMAL) data_writer = csv.writer(outputFile,outputFile, lineterminator='\n') FilterX = simple_filter() FilterY = simple_filter() FilterZ = simple_filter() FilterYaw = simple_filter() LPX97 = simple_decay_filter(0.97) LPY97 = simple_decay_filter(0.97) LPZ97 = simple_decay_filter(0.97) LPYaw97 = simple_decay_filter(0.97) LPX93 = simple_decay_filter(0.93) LPY93 = simple_decay_filter(0.93) LPZ93 = simple_decay_filter(0.93) LPYaw93 = simple_decay_filter(0.93) LPX89 = simple_decay_filter(0.89) LPY89 = simple_decay_filter(0.89) LPZ89 = simple_decay_filter(0.89) LPYaw89 = simple_decay_filter(0.89) lasttime = 0.0 count = 0 for row in data_reader : if(count == 2): data_writer.writerow(outputFieldnames) if(count > 6): # target = (X1, X2, Y1, Y2, Z1, Z2, Yaw1, Yaw2) in m/s and rad # linearSpeed = (Vx, Vy, Vz) in m/s # LinearAcceleration = (Ax, Ay, Az) in m/s/s # AngularSpeed = yawRate in rad/s # dt in seconds # Kalman.next(target, linearSpeed, linearAcceleration, angularSpeed, dt) Measures = [[float(row['target1_x']), float(row['target2_x'])], \ [float(row['target1_y']), float(row['target2_y'])], \ [float(row['target1_z']), float(row['target2_z'])], \ float(row['lasers_yaw'])] linearSpeed = [ float(row['local_vel_x']), \ float(row['local_vel_y']), \ float(row['local_vel_z'])] angularSpeed = [float(row['local_vel_yaw'])] dt = float(row['time']) - lasttime X = FilterX.next(Measures[0], dt, linearSpeed[0]) Y = FilterY.next(Measures[1], dt, linearSpeed[1]) Z = FilterZ.next(Measures[2], dt, linearSpeed[2]) Yaw = FilterYaw.next(Measures[3], dt, angularSpeed[0]) X_low_97 = LPX97.next(X) Y_low_97 = LPY97.next(Y) Z_low_97 = LPZ97.next(Z) Yaw_low_97 = LPYaw97.next(Yaw) X_low_93 = LPX93.next(X) Y_low_93 = LPY93.next(Y) Z_low_93 = LPZ93.next(Z) Yaw_low_93 = LPYaw93.next(Yaw) X_low_89 = LPX89.next(X) Y_low_89 = LPY89.next(Y) Z_low_89 = LPZ89.next(Z) Yaw_low_89 = LPYaw89.next(Yaw) lasttime = float(row['time']) data_writer.writerow([row['time'], row['target1_x'], row['target1_y'], row['target1_z'], \ row['target2_x'], row['target2_y'], row['target2_z'], \ row['lasers_yaw'], \ row['local_vel_x'], row['local_vel_y'], row['local_vel_z'], row['local_vel_yaw'], \ str(X), str(Y), str(Z), str(Yaw),\ str(X_low_97), str(Y_low_97), str(Z_low_97), str(Yaw_low_97), \ str(X_low_93), str(Y_low_93), str(Z_low_93), str(Yaw_low_93), \ str(X_low_89), str(Y_low_89), str(Z_low_89), str(Yaw_low_89) \ ]) count = count +1