[paparazzi-commits] [4413] mag and accel calibration tool

[antoine drouin]

Revision: 4413
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4413
Author:   poine
Date:     2010-01-07 03:57:02 +0000 (Thu, 07 Jan 2010)
Log Message:
-----------
mag and accel calibration tool

Added Paths:
-----------
    paparazzi3/trunk/sw/tools/calibration/calib.py

Added: paparazzi3/trunk/sw/tools/calibration/calib.py
===================================================================
--- paparazzi3/trunk/sw/tools/calibration/calib.py                              
(rev 0)
+++ paparazzi3/trunk/sw/tools/calibration/calib.py      2010-01-07 03:57:02 UTC 
(rev 4413)
@@ -0,0 +1,149 @@
+#
+# My first attempt at python
+# calibrate accelerometer
+#
+
+import re
+import scipy
+from scipy import optimize
+from scipy import linalg
+from pylab import *
+
+#
+# parse the log
+#
+def read_log(ac_id, filename, sensor):
+    f = open(filename, 'r')
+    pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_RAW (\S+) (\S+) 
(\S+)")
+    list_meas = []
+    while 1:
+        line = f.readline().strip()
+        if line == '':
+            break
+        m=re.match(pattern, line)
+        if m:
+            list_meas.append([float(m.group(2)), float(m.group(3)), 
float(m.group(4))])
+    return scipy.array(list_meas)
+
+#
+# select only non-noisy data
+#
+def filter_meas(meas, window_size, noise_threshold):
+    filtered_meas = []
+    filtered_idx = []
+    for i in range(window_size,len(meas)-window_size):
+        noise = meas[i-window_size:i+window_size,:].std(axis=0)
+        if  linalg.norm(noise) < noise_threshold:
+            filtered_meas.append(meas[i,:])
+            filtered_idx.append(i)
+    return scipy.array(filtered_meas), filtered_idx
+
+#
+# initial boundary based calibration
+#
+def get_min_max_guess(meas, scale):
+    max_meas = meas[:,:].max(axis=0)
+    min_meas = meas[:,:].min(axis=0)
+    n = (max_meas + min_meas) / 2
+    sf = 2*scale/(max_meas - min_meas)
+    return scipy.array([n[0], n[1], n[2], sf[0], sf[1], sf[2]])
+
+#
+# scale the set of measurements
+#
+def scale_measurements(meas, p):
+    l_comp = [];
+    l_norm = [];
+    for m in meas[:,]:
+        sm = (m - p[0:3])*p[3:6]
+        l_comp.append(sm)
+        l_norm.append(linalg.norm(sm))
+    return scipy.array(l_comp), scipy.array(l_norm)
+
+#
+# print xml for airframe file
+#
+def print_xml(p, sensor, res):
+    print ""
+    print "<define name=\""+sensor+"_X_NEUTRAL\" 
value=\""+str(int(round(p[0])))+"\"/>"
+    print "<define name=\""+sensor+"_Y_NEUTRAL\" 
value=\""+str(int(round(p[1])))+"\"/>"
+    print "<define name=\""+sensor+"_Z_NEUTRAL\" 
value=\""+str(int(round(p[2])))+"\"/>"
+    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\"/>"
+
+filename = 'log_accel_booz2_a2'
+ac_id = "151"
+if 1:
+    sensor = "ACCEL"
+    sensor_ref = 9.81
+    sensor_res = 10
+    noise_window = 20;
+    noise_threshold = 40;
+else:
+    sensor = "MAG"
+    sensor_ref = 1.
+    sensor_res = 11
+    noise_window = 10;
+    noise_threshold = 1000;
+    
+    
+print "reading file "+filename+" for aircraft "+ac_id+" and sensor "+sensor
+
+measurements = read_log(ac_id, filename, sensor)
+print "found "+str(len(measurements))+" records"
+
+flt_meas, flt_idx = filter_meas(measurements, noise_window, noise_threshold)
+print "remaining "+str(len(flt_meas))+" after low pass"
+
+p0 = get_min_max_guess(flt_meas, sensor_ref)
+cp0, np0 = scale_measurements(flt_meas, p0)
+print "initial guess : "+str(np0.mean())+" "+str(np0.std())
+print p0
+
+
+def err_func(p,meas,y):
+    cp, np = 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 = scale_measurements(flt_meas, p1)
+
+print "optimized guess : "+str(np1.mean())+" "+str(np1.std())
+print p1
+
+print_xml(p1, sensor, sensor_res)
+
+subplot(3,1,1)
+plot(measurements[:,0])
+plot(measurements[:,1])
+plot(measurements[:,2])
+plot(flt_idx, flt_meas[:,0], 'ro')
+plot(flt_idx, flt_meas[:,1], 'ro')
+plot(flt_idx, flt_meas[:,2], 'ro')
+
+subplot(3,2,3)
+plot(cp0[:,0]);
+plot(cp0[:,1]);
+plot(cp0[:,2]);
+plot(-sensor_ref*scipy.ones(len(flt_meas)));
+plot(sensor_ref*scipy.ones(len(flt_meas)));
+
+subplot(3,2,4)
+plot(np0);
+plot(sensor_ref*scipy.ones(len(flt_meas)));
+
+subplot(3,2,5)
+plot(cp1[:,0]);
+plot(cp1[:,1]);
+plot(cp1[:,2]);
+plot(-sensor_ref*scipy.ones(len(flt_meas)));
+plot(sensor_ref*scipy.ones(len(flt_meas)));
+
+subplot(3,2,6)
+plot(np1);
+plot(sensor_ref*scipy.ones(len(flt_meas)));
+
+show();
+