def process_gprmc(time, row): global gprmc_time, gprmc_groundspeed_ms, gprmc_vario_ms true_groundspeed_kts = measurement(0.0, 2.2035) try: true_groundspeed_kts = measurement(float(row[11]), 10.0/kts2ms) true_course_deg = measurement(float(row[12]), 0.0) except: return true_course_rad = (true_course_deg/180.0)*math.pi true_groundspeed_ms = true_groundspeed_kts * kts2ms #Update kinetic vario dtime = time - gprmc_time gprmc_vario_ms = (true_groundspeed_ms - gprmc_groundspeed_ms)/dtime gprmc_time = time gprmc_groundspeed_ms = true_groundspeed_ms #Sin/cos swapped because of 90deg difference groundspeed_x = measurement(true_groundspeed_ms.get_mean() * math.sin(true_course_rad.get_mean()), 10.0) groundspeed_y = measurement(true_groundspeed_ms.get_mean() * math.cos(true_course_rad.get_mean()), 10.0) [wind_x, wind_y, wind_x0, wind_y0, wind_x1, wind_y1, wind_x2, wind_y2] = calc_wind(groundspeed_x, groundspeed_y) #[measurement(-2.28, 1.0), measurement(-1.46, 1.0)] kalman.update_true_groundspeed(time, groundspeed_x, groundspeed_y) kalman.update_wind(time, wind_x, wind_y) line = ",".join([str(time), csv_meas(groundspeed_x), csv_meas(groundspeed_y), csv_meas(wind_x), csv_meas(wind_y), csv_meas(kalman.get_wind_x().predict(time, 0.0)), csv_meas(kalman.get_wind_y().predict(time, 0.0)), csv_meas(wind_x0), csv_meas(wind_y0), csv_meas(wind_x1), csv_meas(wind_y1), csv_meas(wind_x2), csv_meas(wind_y2)])
def process_bmp085(time, row): global bmp085_time, bmp085_height try: static_pressure = measurement(float(row[4]), 6.362254) except: return height = measurement(pressure2height(static_pressure.get_mean()), 0.4533) vario = (height - bmp085_height)/(time - bmp085_time) kalman.update_static_pressure(time, static_pressure) kalman.update_alt_vario(time, height, vario) bmp085_time = time bmp085_height = height
def calc_safety_glide(a,b,c,height,distance,probability,uncertainty): target_uncertainty = math.sqrt(distance) * uncertainty target = measurement(target_height, target_uncertainty) safety_target_height = calc_probability_height(target, probability) safety_delta_height = height - safety_target_height safety_glide_ratio = distance/safety_delta_height velocity = calc_speed_from_glide_ratio(a,b,c,safety_glide_ratio) if ((target < safety_target_height) - probability) > 0.01: exit("SANITY CHECK FAILED") return velocity
def process_mma8452(time, row): try: g0 = float(row[10]) g1 = float(row[11]) g2 = float(row[12]) except: return g = math.sqrt((g0*g0) + (g1*g1) + (g2*g2)) g_meas = measurement(g, 0.10567) kalman.update_g(time, g_meas)
def process_ms5803_01ba_77(time, row): global ms5803_01ba_77_height, ms5803_01ba_77_time # try: pressure = float(row[12]) # except: # return height = pressure2height(pressure) height_meas = measurement(height, 0.21) vario = (height_meas - ms5803_01ba_77_height)/(time - ms5803_01ba_77_time) kalman.update_alt_vario(time, height_meas, vario) ms5803_01ba_77_time = time ms5803_01ba_77_height = height_meas
def calc_wind(wind_x2, wind_y2): global wind_x_backlog, wind_y_backlog wind_x_backlog.append(wind_x2) wind_y_backlog.append(wind_y2) wind_len = len(wind_x_backlog) wind_x0 = wind_x_backlog[0] wind_y0 = wind_y_backlog[0] wind_x1 = wind_x_backlog[wind_len/2] wind_y1 = wind_y_backlog[wind_len/2] if wind_len < 3: return [measurement(0.0, finf), measurement(0.0, finf),wind_x0,wind_y0,wind_x1,wind_y1,wind_x2,wind_y2] [m12_x, m12_y] = midpoint(wind_x0, wind_y0, wind_x1, wind_y1) [m23_x, m23_y] = midpoint(wind_x1, wind_y1, wind_x2, wind_y2) [r12_x, r12_y] = perpendicular(wind_x0, wind_y0, wind_x1, wind_y1) [r23_x, r23_y] = perpendicular(wind_x1, wind_y1, wind_x2, wind_y2) teller = (-r23_x * m23_y) - (r23_y * m12_x) + (r23_y * m23_x) + (m12_y * r23_x) noemer = (r23_y * r12_x) - (r12_y * r23_x) try: v = teller/noemer except ZeroDivisionError: v = measurement(math.copysign(finf, teller.get_mean()), finf) x = m12_x + (v * r12_x) y = m12_y + (v * r12_y) if wind_len > (20*22): wind_x_backlog.pop(0) wind_y_backlog.pop(0) return [x,y,wind_x0,wind_y0,wind_x1,wind_y1,wind_x2,wind_y2]
def process_gpgga(time, row): global gpgga_time, gpgga_height try: fix_type = int(row[10]) if fix_type > 0: hdop = float(row[12]) height = measurement(float(row[13]), 10.0*hdop) vario = (height - gpgga_height)/(time - gpgga_time) except: return if fix_type > 0: kalman.update_alt_vario(time, height, vario) gpgga_time = time gpgga_height = height
end = {} seconds = {} pascal = {} height = {} dtime = {} height_prediction = {} height_kalman = {} vario = {} vario_prediction = {} vario_kalman = {} g_limit = 0.5 vario_process_noise = measurement(0, ((9.81*g_limit)/3)) #Acceleration is 20m/s^2 99% probability (3-sigma). height_process_noise = measurement(0, 0) with sys.stdin as csvfile: metingen = csv.DictReader(csvfile, delimiter=',') i = 0 for row in metingen: #Read data start_sec[i] = float(row["Start"]) start_nsec[i] = float(row["Start_nsec"]) end_sec[i] = float(row["End"]) end_nsec[i] = float(row["End_nsec"]) pascal[i] = float(row["Pascal"]) #Construct start time and end time
v = teller/noemer except ZeroDivisionError: v = measurement(math.copysign(finf, teller.get_mean()), finf) x = m12_x + (v * r12_x) y = m12_y + (v * r12_y) if wind_len > (20*22): wind_x_backlog.pop(0) wind_y_backlog.pop(0) return [x,y,wind_x0,wind_y0,wind_x1,wind_y1,wind_x2,wind_y2] bmp085_time = 0.0 bmp085_height = measurement(0.0, 0.0) def process_bmp085(time, row): global bmp085_time, bmp085_height try: static_pressure = measurement(float(row[4]), 6.362254) except: return height = measurement(pressure2height(static_pressure.get_mean()), 0.4533) vario = (height - bmp085_height)/(time - bmp085_time) kalman.update_static_pressure(time, static_pressure) kalman.update_alt_vario(time, height, vario) bmp085_time = time bmp085_height = height
velocity_ms = {} velocity_ms_prediction = {} velocity_ms_kalman = {} accel_hor = {} accel_hor_prediction = {} accel_hor_kalman = {} dtime = {} g=9.81 g_limit = 1.0 knots2ms = 0.514444 velocity_process_noise = measurement(0, g*g_limit/3) accel_hor_process_noise = measurement(0, g*g_limit/3) with sys.stdin as csvfile: metingen = csv.reader(csvfile, delimiter=',') i = 0 for row in metingen: #Read data start_sec[i] = float(row[0]) start_nsec[i] = float(row[1]) end_sec[i] = float(row[2]) end_nsec[i] = float(row[3]) try: velocity_knots[i] = measurement(float(row[11]), 2.2035) #Value from stdev(L1:L12000)
end = {} dtime = {} orientation = {} rotation = {} orientation_prediction = {} with sys.stdin as csvfile: metingen = csv.DictReader(csvfile, delimiter=',') i = 0 for row in metingen: start_sec[i] = float(row["Start_sec"]) start_nsec[i] = float(row["Start_nsec"]) end_sec[i] = float(row["End_sec"]) end_nsec[i] = float(row["End_nsec"]) accel_x[i] = measurement(float(row["A_x"]), 1.21) accel_y[i] = measurement(float(row["A_y"]), 7.25) accel_z[i] = measurement(float(row["A_z"]), 1.97) rot_x[i] = measurement(float(row["Rot_x"]), 0.94) rot_y[i] = measurement(float(row["Rot_y"]), 1.40) rot_z[i] = measurement(float(row["Rot_z"]), 0.89) mag_x[i] = measurement(float(row["Mag_x"]), 3.90) mag_y[i] = measurement(float(row["Mag_y"]), 14.92) mag_z[i] = measurement(float(row["Mag_z"]), 5.91) start[i] = start_sec[i] + (start_nsec[i] * math.pow(10, -9)) end[i] = end_sec[i] + (end_nsec[i] * math.pow(10, -9)) #Time if i > 0: dtime[i] = ((start[i] - start[i-1]) + (end[i] - end[i - 1]) / 2) else: dtime[i] = 1
end = {} seconds = {} hdop = {} height = {} dtime = {} height_prediction = {} height_kalman = {} vario = {} vario_prediction = {} vario_kalman = {} g_limit = 1.7 vario_process_noise = measurement(0, ((9.81*g_limit)/3.0)) #Acceleration is 20m/s^2 99% probability (3-sigma). height_process_noise = measurement(0, 0.1/3.0) with sys.stdin as csvfile: metingen = csv.reader(csvfile, delimiter=',') i = 0 for row in metingen: #Read data start_sec[i] = float(row[0]) start_nsec[i] = float(row[1]) end_sec[i] = float(row[2]) end_nsec[i] = float(row[3]) hdop[i] = float(row[12]) if hdop[i] == 0.0: hdop[i] = 1.0 height[i] = measurement(float(row[13]), hdop[i]*10.0) #bmp085 value for lack of something better