def calibrate(self):
self.calc_min_max_guess()
if len(self.flt_meas) < 10:
logging.warning("Not enough measurements")
return
cp0, np0 = calibration_utils.scale_measurements(self.flt_meas, self.p0)
logging.info("initial guess : avg "+str(np0.mean())+" std "+str(np0.std()))
calibration_utils.print_xml(self.p0, "MAG", 11)
def err_func(p, meas, y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y * scipy.ones(len(meas)) - np
return err
p1, cov, info, msg, success = optimize.leastsq(err_func, self.p0[:], args=(self.flt_meas, 1.0), full_output=1)
self.optimization_done = success in [1, 2, 3, 4]
if not self.optimization_done:
logging.warning("Optimization error: ", msg)
cp1, np1 = calibration_utils.scale_measurements(self.flt_meas, p1)
if self.optimization_done:
logging.info("optimized guess : avg " + str(np1.mean()) + " std " + str(np1.std()))
calibration_utils.print_xml(p1, "MAG", 11)
else:
logging.info("last iteration of failed optimized guess : avg " + str(np1.mean()) + " std " + str(np1.std()))
if self.plot_results:
calibration_utils.plot_results("MAG", np.array(self.data), range(len(self.data)),
self.flt_meas, cp0, np0, cp1, np1, 1.0, blocking=False)
calibration_utils.plot_mag_3d(self.flt_meas, cp1, p1)
def calibrate(self):
self.calc_min_max_guess()
if len(self.flt_meas) < 10:
logging.warning("Not enough measurements")
return
cp0, np0 = calibration_utils.scale_measurements(self.flt_meas, self.p0)
logging.info("initial guess : avg "+str(np0.mean())+" std "+str(np0.std()))
calibration_utils.print_xml(self.p0, "MAG", 11)
def err_func(p, meas, y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y * scipy.ones(len(meas)) - np
return err
p1, cov, info, msg, success = optimize.leastsq(err_func, self.p0[:], args=(self.flt_meas, 1.0), full_output=1)
self.optimization_done = success in [1, 2, 3, 4]
if not self.optimization_done:
logging.warning("Optimization error: ", msg)
cp1, np1 = calibration_utils.scale_measurements(self.flt_meas, p1)
if self.optimization_done:
logging.info("optimized guess : avg " + str(np1.mean()) + " std " + str(np1.std()))
calibration_utils.print_xml(p1, "MAG", 11)
else:
logging.info("last iteration of failed optimized guess : avg " + str(np1.mean()) + " std " + str(np1.std()))
if self.plot_results:
calibration_utils.plot_results("MAG", np.array(self.data), range(len(self.data)),
self.flt_meas, cp0, np0, cp1, np1, 1.0, blocking=False)
calibration_utils.plot_mag_3d(self.flt_meas, cp1, p1)
def main():
usage = "usage: %prog [options] log_filename.data" + "\n" + "Run %prog --help to list the options."
parser = OptionParser(usage)
parser.add_option("-i", "--id", dest="ac_id",
action="store",
help="aircraft id to use")
parser.add_option("-s", "--sensor", dest="sensor",
type="choice", choices=["ACCEL", "MAG"],
help="sensor to calibrate (ACCEL, MAG)",
action="store", default="ACCEL")
parser.add_option("-p", "--plot",
help="Show resulting plots",
action="store_true", dest="plot")
parser.add_option("--noise_threshold",
help="specify noise threshold instead of automatically determining it",
action="store", dest="noise_threshold", default=0)
parser.add_option("-v", "--verbose",
action="store_true", dest="verbose")
(options, args) = parser.parse_args()
if len(args) != 1:
parser.error("incorrect number of arguments")
else:
if os.path.isfile(args[0]):
filename = args[0]
else:
print(args[0] + " not found")
sys.exit(1)
if not filename.endswith(".data"):
parser.error("Please specify a *.data log file")
if os.path.getsize(filename) == 0:
print("File specified has no data.")
sys.exit(1)
ac_ids = calibration_utils.get_ids_in_log(filename)
if options.ac_id is None:
if len(ac_ids) == 1:
options.ac_id = ac_ids[0]
else:
parser.error("More than one aircraft id found in log file. Specify the id to use.")
if options.verbose:
print("Using aircraft id "+options.ac_id)
if options.sensor == "ACCEL":
sensor_ref = 9.81
sensor_res = 10
noise_window = 20
noise_threshold = options.noise_threshold
elif options.sensor == "MAG":
sensor_ref = 1.
sensor_res = 11
noise_window = 10
noise_threshold = 1000
if options.verbose:
print("reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor)
# read raw measurements from log file
measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor)
if len(measurements) == 0:
print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file!")
sys.exit(1)
if options.verbose:
print("found "+str(len(measurements))+" records")
# check that values are not all zero
if not measurements.any():
print("Error: all IMU_"+options.sensor+"_RAW measurements are zero!")
sys.exit(1)
# estimate the noise threshold if not explicitly given
if noise_threshold <= 0:
# mean over all measurements (flattended array) as approx neutral value
neutral = scipy.mean(measurements)
# find the median of measurement vector length after subtracting approximate neutral
meas_median = scipy.median(scipy.array([scipy.linalg.norm(v - neutral) for v in measurements]))
# set noise threshold to be below 10% of that
noise_threshold = meas_median * 0.1
if options.verbose:
print("Using noise threshold of", noise_threshold, "for filtering.")
# filter out noisy measurements
flt_meas, flt_idx = calibration_utils.filter_meas(measurements, noise_window, noise_threshold)
if options.verbose:
print("remaining "+str(len(flt_meas))+" after filtering")
if len(flt_meas) == 0:
print("Error: found zero IMU_" + options.sensor + "_RAW measurements for aircraft with id " + options.ac_id +
" in log file after filtering with noise threshold of " + noise_threshold +
"!\nMaybe try specifying manually with the --noise_threshold option.")
if options.plot:
calibration_utils.plot_measurements(options.sensor, measurements)
sys.exit(1)
# get an initial min/max guess
p0 = calibration_utils.get_min_max_guess(flt_meas, sensor_ref)
cp0, np0 = calibration_utils.scale_measurements(flt_meas, p0)
print("initial guess : avg "+str(np0.mean())+" std "+str(np0.std()))
# print p0
def err_func(p, meas, y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y*scipy.ones(len(meas)) - np
return err
p1, cov, info, msg, success = optimize.leastsq(err_func, p0[:], args=(flt_meas, sensor_ref), full_output=1)
optimze_failed = success not in [1, 2, 3, 4]
if optimze_failed:
print("Optimization error: ", msg)
print("Please try to provide a clean logfile with proper distribution of measurements.")
#sys.exit(1)
cp1, np1 = calibration_utils.scale_measurements(flt_meas, p1)
if optimze_failed:
print("last iteration of failed optimized guess : avg "+str(np1.mean())+" std "+str(np1.std()))
else:
print("optimized guess : avg "+str(np1.mean())+" std "+str(np1.std()))
if not optimze_failed:
calibration_utils.print_xml(p1, options.sensor, sensor_res)
if options.plot:
# if we are calibrating a mag, just draw first plot (non-blocking), then show the second
if options.sensor == "MAG":
calibration_utils.plot_results(options.sensor, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref, blocking=False)
calibration_utils.plot_mag_3d(flt_meas, cp1, p1)
# otherwise show the first plot (blocking)
else:
calibration_utils.plot_results(options.sensor, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
def err_func(p, meas, y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y*scipy.ones(len(meas)) - np
return err
def main():
usage = "usage: %prog [options] log_filename.data" + "\n" + "Run %prog --help to list the options."
parser = OptionParser(usage)
parser.add_option("-i", "--id", dest="ac_id",
action="store",
help="aircraft id to use")
parser.add_option("-s", "--sensor", dest="sensor",
type="choice", choices=["ACCEL", "MAG"],
help="sensor to calibrate (ACCEL, MAG)",
action="store", default="ACCEL")
parser.add_option("-p", "--plot",
help="Show resulting plots",
action="store_true", dest="plot")
parser.add_option("-a", "--auto_threshold",
help="Try to automatically determine noise threshold",
action="store_true", dest="auto_threshold")
parser.add_option("-v", "--verbose",
action="store_true", dest="verbose")
(options, args) = parser.parse_args()
if len(args) != 1:
parser.error("incorrect number of arguments")
else:
if os.path.isfile(args[0]):
filename = args[0]
else:
print(args[0] + " not found")
sys.exit(1)
ac_ids = calibration_utils.get_ids_in_log(filename)
if options.ac_id == None:
if len(ac_ids) == 1:
options.ac_id = ac_ids[0]
else:
parser.error("More than one aircraft id found in log file. Specify the id to use.")
if options.verbose:
print("Using aircraft id "+options.ac_id)
if options.sensor == "ACCEL":
sensor_ref = 9.81
sensor_res = 10
noise_window = 20;
noise_threshold = 40;
elif options.sensor == "MAG":
sensor_ref = 1.
sensor_res = 11
noise_window = 10;
noise_threshold = 1000;
if not filename.endswith(".data"):
parser.error("Please specify a *.data log file")
if options.verbose:
print("reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor)
# read raw measurements from log file
measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor)
if len(measurements) == 0:
print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file!")
sys.exit(1)
if options.verbose:
print("found "+str(len(measurements))+" records")
# estimate the noise threshold
# find the median of measurement vector lenght
if options.auto_threshold:
meas_median = scipy.median(scipy.array([scipy.linalg.norm(v) for v in measurements]))
# set noise threshold to be below 10% of that
noise_threshold = meas_median * 0.1
if options.verbose:
print("Using noise threshold of", noise_threshold, "for filtering.")
# filter out noisy measurements
flt_meas, flt_idx = calibration_utils.filter_meas(measurements, noise_window, noise_threshold)
if options.verbose:
print("remaining "+str(len(flt_meas))+" after filtering")
if len(flt_meas) == 0:
print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file after filtering!")
sys.exit(1)
# get an initial min/max guess
p0 = calibration_utils.get_min_max_guess(flt_meas, sensor_ref)
cp0, np0 = calibration_utils.scale_measurements(flt_meas, p0)
print("initial guess : avg "+str(np0.mean())+" std "+str(np0.std()))
# print p0
def err_func(p, meas, y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y*scipy.ones(len(meas)) - np
return err
p1, success = optimize.leastsq(err_func, p0[:], args=(flt_meas, sensor_ref))
cp1, np1 = calibration_utils.scale_measurements(flt_meas, p1)
print("optimized guess : avg "+str(np1.mean())+" std "+str(np1.std()))
# print p1
calibration_utils.print_xml(p1, options.sensor, sensor_res)
print("")
if options.plot:
# if we are calibrating a mag, just draw first plot (non-blocking), then show the second
if options.sensor == "MAG":
calibration_utils.plot_results(False, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
calibration_utils.plot_mag_3d(flt_meas, cp1, p1)
# otherwise show the first plot (blocking)
else:
calibration_utils.plot_results(True, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
def main():
usage = "usage: %prog [options] log_filename.data" + "\n" + "Run %prog --help to list the options."
parser = OptionParser(usage)
parser.add_option("-i",
"--id",
dest="ac_id",
action="store",
help="aircraft id to use")
parser.add_option("-s",
"--sensor",
dest="sensor",
type="choice",
choices=["ACCEL", "MAG"],
help="sensor to calibrate (ACCEL, MAG)",
action="store",
default="ACCEL")
parser.add_option("-p",
"--plot",
help="Show resulting plots",
action="store_true",
dest="plot")
parser.add_option("-v", "--verbose", action="store_true", dest="verbose")
(options, args) = parser.parse_args()
if len(args) != 1:
parser.error("incorrect number of arguments")
else:
if os.path.isfile(args[0]):
filename = args[0]
else:
print(args[0] + " not found")
sys.exit(1)
ac_ids = calibration_utils.get_ids_in_log(filename)
if options.ac_id == None:
if len(ac_ids) == 1:
options.ac_id = ac_ids[0]
else:
parser.error(
"More than one aircraft id found in log file. Specify the id to use."
)
if options.verbose:
print("Using aircraft id " + options.ac_id)
if options.sensor == "ACCEL":
sensor_ref = 9.81
sensor_res = 10
noise_window = 20
noise_threshold = 40
elif options.sensor == "MAG":
sensor_ref = 1.
sensor_res = 11
noise_window = 10
noise_threshold = 1000
if not filename.endswith(".data"):
parser.error("Please specify a *.data log file")
if options.verbose:
print("reading file " + filename + " for aircraft " + options.ac_id +
" and sensor " + options.sensor)
# read raw measurements from log file
measurements = calibration_utils.read_log(options.ac_id, filename,
options.sensor)
if len(measurements) == 0:
print("Error: found zero IMU_" + options.sensor +
"_RAW measurements for aircraft with id " + options.ac_id +
" in log file!")
sys.exit(1)
if options.verbose:
print("found " + str(len(measurements)) + " records")
# filter out noisy measurements
flt_meas, flt_idx = calibration_utils.filter_meas(measurements,
noise_window,
noise_threshold)
if options.verbose:
print("remaining " + str(len(flt_meas)) + " after low pass")
if len(flt_meas) == 0:
print("Error: found zero IMU_" + options.sensor +
"_RAW measurements for aircraft with id " + options.ac_id +
" in log file after low pass!")
sys.exit(1)
# get an initial min/max guess
p0 = calibration_utils.get_min_max_guess(flt_meas, sensor_ref)
cp0, np0 = calibration_utils.scale_measurements(flt_meas, p0)
print("initial guess : avg " + str(np0.mean()) + " std " + str(np0.std()))
# print p0
def err_func(p, meas, y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y * scipy.ones(len(meas)) - np
return err
p1, success = optimize.leastsq(err_func,
p0[:],
args=(flt_meas, sensor_ref))
cp1, np1 = calibration_utils.scale_measurements(flt_meas, p1)
print("optimized guess : avg " + str(np1.mean()) + " std " +
str(np1.std()))
# print p1
calibration_utils.print_xml(p1, options.sensor, sensor_res)
print("")
if options.plot:
# if we are calibrating a mag, just draw first plot (non-blocking), then show the second
if options.sensor == "MAG":
calibration_utils.plot_results(False, measurements, flt_idx,
flt_meas, cp0, np0, cp1, np1,
sensor_ref)
calibration_utils.plot_mag_3d(flt_meas, cp1, p1)
# otherwise show the first plot (blocking)
else:
calibration_utils.plot_results(True, measurements, flt_idx,
flt_meas, cp0, np0, cp1, np1,
sensor_ref)
def main():
usage = "usage: %prog [options] log_filename"
parser = OptionParser(usage)
parser.add_option("-i", "--id", dest="ac_id",
action="store",
help="aircraft id to use")
parser.add_option("-s", "--sensor", dest="sensor",
help="sensor to calibrate ( ACCEL or MAG)",
action="store", default="ACCEL")
parser.add_option("-v", "--verbose",
action="store_true", dest="verbose")
(options, args) = parser.parse_args()
if len(args) != 1:
parser.error("incorrect number of arguments")
else:
if os.path.isfile(args[0]):
filename = args[0]
else:
print args[0] + " not found"
sys.exit(1)
if options.sensor == "GYRO":
print "You can't calibate gyros with this!"
sys.exit(1)
ac_ids = calibration_utils.get_ids_in_log(filename)
# import code; code.interact(local=locals())
if options.ac_id == None and len(ac_ids) == 1:
options.ac_id = ac_ids[0]
if options.verbose:
print "reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor
measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor)
if options.verbose:
print "found "+str(len(measurements))+" records"
if options.sensor == "ACCEL":
sensor_ref = 9.81
sensor_res = 10
noise_window = 20;
noise_threshold = 40;
else: # MAG
sensor_ref = 1.
sensor_res = 11
noise_window = 10;
noise_threshold = 1000;
flt_meas, flt_idx = calibration_utils.filter_meas(measurements, noise_window, noise_threshold)
if options.verbose:
print "remaining "+str(len(flt_meas))+" after low pass"
p0 = calibration_utils.get_min_max_guess(flt_meas, sensor_ref)
cp0, np0 = calibration_utils.scale_measurements(flt_meas, p0)
print "initial guess : avg "+str(np0.mean())+" std "+str(np0.std())
# print p0
def err_func(p,meas,y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y*scipy.ones(len(meas)) - np
return err
p1, success = optimize.leastsq(err_func, p0[:], args=(flt_meas, sensor_ref))
cp1, np1 = calibration_utils.scale_measurements(flt_meas, p1)
print "optimized guess : avg "+str(np1.mean())+" std "+str(np1.std())
# print p1
calibration_utils.print_xml(p1, options.sensor, sensor_res)
print ""
calibration_utils.plot_results(measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
def main():
usage = "usage: %prog [options] log_filename"
parser = OptionParser(usage)
parser.add_option("-i",
"--id",
dest="ac_id",
action="store",
help="aircraft id to use")
parser.add_option("-s",
"--sensor",
dest="sensor",
help="sensor to calibrate ( ACCEL or MAG)",
action="store",
default="ACCEL")
parser.add_option("-v", "--verbose", action="store_true", dest="verbose")
(options, args) = parser.parse_args()
if len(args) != 1:
parser.error("incorrect number of arguments")
else:
if os.path.isfile(args[0]):
filename = args[0]
else:
print args[0] + " not found"
sys.exit(1)
ac_ids = calibration_utils.get_ids_in_log(filename)
# import code; code.interact(local=locals())
if options.ac_id == None and len(ac_ids) == 1:
options.ac_id = ac_ids[0]
if options.verbose:
print "reading file " + filename + " for aircraft " + options.ac_id + " and sensor " + options.sensor
measurements = calibration_utils.read_log(options.ac_id, filename,
options.sensor)
if options.verbose:
print "found " + str(len(measurements)) + " records"
if options.sensor == "ACCEL":
sensor_ref = 9.81
sensor_res = 10
noise_window = 20
noise_threshold = 40
else: # MAG
sensor_ref = 1.
sensor_res = 11
noise_window = 10
noise_threshold = 1000
flt_meas, flt_idx = calibration_utils.filter_meas(measurements,
noise_window,
noise_threshold)
if options.verbose:
print "remaining " + str(len(flt_meas)) + " after low pass"
p0 = calibration_utils.get_min_max_guess(flt_meas, sensor_ref)
cp0, np0 = calibration_utils.scale_measurements(flt_meas, p0)
print "initial guess : avg " + str(np0.mean()) + " std " + str(np0.std())
# print p0
def err_func(p, meas, y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y * scipy.ones(len(meas)) - np
return err
p1, success = optimize.leastsq(err_func,
p0[:],
args=(flt_meas, sensor_ref))
cp1, np1 = calibration_utils.scale_measurements(flt_meas, p1)
print "optimized guess : avg " + str(np1.mean()) + " std " + str(np1.std())
# print p1
calibration_utils.print_xml(p1, options.sensor, sensor_res)
print ""
calibration_utils.plot_results(measurements, flt_idx, flt_meas, cp0, np0,
cp1, np1, sensor_ref)