[tools] calibration: automatically choose noise_threshold

sw/tools/calibration/calibrate.py

@@ -20,6 +20,7 @@
 # Boston, MA 02111-1307, USA.
 #
 
+from __future__ import print_function
 
 import sys
 import os
@@ -66,12 +67,12 @@ def main():
         sensor_ref = 9.81
         sensor_res = 10
         noise_window = 20;
-        noise_threshold = 40;
+        #noise_threshold = 40;
     elif options.sensor == "MAG":
         sensor_ref = 1.
         sensor_res = 11
         noise_window = 10;
-        noise_threshold = 1000;
+        #noise_threshold = 1000;
 
     if not filename.endswith(".data"):
         parser.error("Please specify a *.data log file")
@@ -86,12 +87,20 @@ def main():
     if options.verbose:
        print("found "+str(len(measurements))+" records")
 
+    # estimate the noise threshold
+    # find the median of measurement vector lenght
+    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 low pass")
+        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 low pass!")
+        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