diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py
index 5e7f70e535..2d96623aa4 100755
--- a/sw/tools/calibration/calibrate.py
+++ b/sw/tools/calibration/calibrate.py
@@ -58,6 +58,10 @@ def main():
         else:
             print(args[0] + " not found")
             sys.exit(1)
+
+    if not filename.endswith(".data"):
+        parser.error("Please specify a *.data log file")
+
     ac_ids = calibration_utils.get_ids_in_log(filename)
     if options.ac_id is None:
         if len(ac_ids) == 1:
@@ -78,8 +82,6 @@ def main():
         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)
 
@@ -120,27 +122,30 @@ def main():
         return err
 
     p1, cov, info, msg, success = optimize.leastsq(err_func, p0[:], args=(flt_meas, sensor_ref), full_output=1)
-    if not success in [1, 2, 3, 4]:
+    optimze_failed = success not in [1, 2, 3, 4]
+    if optimze_failed:
         print("Optimization error: ", msg)
-        print("Please try to provide a clean logfile.")
-        sys.exit(1)
+        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)
 
-    print("optimized guess : avg "+str(np1.mean())+" std "+str(np1.std()))
-#    print 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()))
 
-    calibration_utils.print_xml(p1, options.sensor, sensor_res)
-    print("")
+    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(False, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
+            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(True, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
+            calibration_utils.plot_results(options.sensor, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
 
 if __name__ == "__main__":
     main()
diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py
index cabc3601bb..3614c179aa 100644
--- a/sw/tools/calibration/calibration_utils.py
+++ b/sw/tools/calibration/calibration_utils.py
@@ -144,6 +144,7 @@ def print_xml(p, sensor, res):
     print("<define name=\""+sensor+"_X_SENS\" value=\""+str(p[3]*2**res)+"\" integer=\"16\"/>")
     print("<define name=\""+sensor+"_Y_SENS\" value=\""+str(p[4]*2**res)+"\" integer=\"16\"/>")
     print("<define name=\""+sensor+"_Z_SENS\" value=\""+str(p[5]*2**res)+"\" integer=\"16\"/>")
+    print("")
 
 
 def print_imu_scaled(sensor, measurements, attrs):
@@ -158,8 +159,9 @@ def print_imu_scaled(sensor, measurements, attrs):
 
 
 
-def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref):
+def plot_results(sensor, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref, blocking=True):
     """Plot calibration results."""
+    # plot raw measurements with filtered ones marked as red circles
     plt.subplot(3, 1, 1)
     plt.plot(measurements[:, 0])
     plt.plot(measurements[:, 1])
@@ -167,43 +169,52 @@ def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sen
     plt.plot(flt_idx, flt_meas[:, 0], 'ro')
     plt.plot(flt_idx, flt_meas[:, 1], 'ro')
     plt.plot(flt_idx, flt_meas[:, 2], 'ro')
-    plt.xlabel('time (s)')
     plt.ylabel('ADC')
-    plt.title('Raw sensors')
+    plt.title('Raw '+sensor+', red dots are actually used measurements')
 
+    plt.tight_layout()
+    # show scaled measurements with initial guess
     plt.subplot(3, 2, 3)
     plt.plot(cp0[:, 0])
     plt.plot(cp0[:, 1])
     plt.plot(cp0[:, 2])
     plt.plot(-sensor_ref*np.ones(len(flt_meas)))
     plt.plot(sensor_ref*np.ones(len(flt_meas)))
+    plt.title('scaled '+sensor+' (initial guess)')
+    plt.xticks([])
 
     plt.subplot(3, 2, 4)
     plt.plot(np0)
     plt.plot(sensor_ref*np.ones(len(flt_meas)))
+    plt.title('norm of '+sensor+' (initial guess)')
+    plt.xticks([])
 
+    # show scaled measurements after optimization
     plt.subplot(3, 2, 5)
     plt.plot(cp1[:, 0])
     plt.plot(cp1[:, 1])
     plt.plot(cp1[:, 2])
     plt.plot(-sensor_ref*np.ones(len(flt_meas)))
     plt.plot(sensor_ref*np.ones(len(flt_meas)))
+    plt.title('scaled '+sensor+' (optimized)')
+    plt.xticks([])
 
     plt.subplot(3, 2, 6)
     plt.plot(np1)
     plt.plot(sensor_ref*np.ones(len(flt_meas)))
+    plt.title('norm of '+sensor+' (optimized)')
+    plt.xticks([])
 
     # if we want to have another plot we only draw the figure (non-blocking)
     # also in matplotlib before 1.0.0 there is only one call to show possible
-    if block:
+    if blocking:
         plt.show()
     else:
         plt.draw()
 
+
 def plot_imu_scaled(sensor, measurements, attrs):
     """Plot imu scaled results."""
-
-
     plt.figure("Sensor Scaled")
 
     plt.subplot(4, 1, 1)
@@ -231,9 +242,9 @@ def plot_imu_scaled(sensor, measurements, attrs):
 
     plt.show()
 
+
 def plot_imu_scaled_fft(sensor, measurements, attrs):
     """Plot imu scaled fft results."""
-
     #dt = 0.0769
     #Fs = 1/dt
     Fs = 26.0