[paparazzi-commits] [4893] toying around with calibration

[antoine drouin]

Revision: 4893
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4893
Author:   poine
Date:     2010-04-29 20:03:32 +0000 (Thu, 29 Apr 2010)
Log Message:
-----------
toying around with calibration

Modified Paths:
--------------
    paparazzi3/trunk/sw/tools/calibration/calibrate.py

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

Removed Paths:
-------------
    paparazzi3/trunk/sw/tools/calibration/calibrate_gui.py
    paparazzi3/trunk/sw/tools/calibration/sensor_calibration.py

Modified: paparazzi3/trunk/sw/tools/calibration/calibrate.py
===================================================================
--- paparazzi3/trunk/sw/tools/calibration/calibrate.py  2010-04-29 12:31:21 UTC 
(rev 4892)
+++ paparazzi3/trunk/sw/tools/calibration/calibrate.py  2010-04-29 20:03:32 UTC 
(rev 4893)
@@ -28,7 +28,7 @@
 import scipy
 from scipy import optimize
 
-import sensor_calibration
+import calibration_utils
 
 
 def main():
@@ -51,13 +51,13 @@
         else:
             print args[0] + " not found"
             sys.exit(1)
-    ac_ids = sensor_calibration.get_ids_in_log(filename)
+    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 = sensor_calibration.read_log(options.ac_id, filename, 
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":
@@ -70,29 +70,29 @@
         sensor_res = 11
         noise_window = 10;
         noise_threshold = 1000;
-    flt_meas, flt_idx = sensor_calibration.filter_meas(measurements, 
noise_window, noise_threshold)
+    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 = sensor_calibration.get_min_max_guess(flt_meas, sensor_ref)
-    cp0, np0 = sensor_calibration.scale_measurements(flt_meas, p0)
+    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 = sensor_calibration.scale_measurements(meas, p)
+        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 = sensor_calibration.scale_measurements(flt_meas, p1)
+    cp1, np1 = calibration_utils.scale_measurements(flt_meas, p1)
 
     print "optimized guess : avg "+str(np1.mean())+" std "+str(np1.std())
 #    print p1
 
-    sensor_calibration.print_xml(p1, options.sensor, sensor_res)
+    calibration_utils.print_xml(p1, options.sensor, sensor_res)
     print ""
 
-    sensor_calibration.plot_results(measurements, flt_idx, flt_meas, cp0, np0, 
cp1, np1, sensor_ref)
+    calibration_utils.plot_results(measurements, flt_idx, flt_meas, cp0, np0, 
cp1, np1, sensor_ref)
 
 if __name__ == "__main__":
     main()

Deleted: paparazzi3/trunk/sw/tools/calibration/calibrate_gui.py
===================================================================
--- paparazzi3/trunk/sw/tools/calibration/calibrate_gui.py      2010-04-29 
12:31:21 UTC (rev 4892)
+++ paparazzi3/trunk/sw/tools/calibration/calibrate_gui.py      2010-04-29 
20:03:32 UTC (rev 4893)
@@ -1,103 +0,0 @@
-#!/usr/bin/env python
-
-#  $Id$
-#  Copyright (C) 2010 Antoine Drouin
-#
-# This file is part of Paparazzi.
-#
-# Paparazzi is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2, or (at your option)
-# any later version.
-#
-# Paparazzi is distributed in the hope that it will be useful,
-# but WITHOUT ANY WARRANTY; without even the implied warranty of
-# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-# GNU General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with Paparazzi; see the file COPYING.  If not, write to
-# the Free Software Foundation, 59 Temple Place - Suite 330,
-# Boston, MA 02111-1307, USA.  
-#
-
-import pygtk
-pygtk.require('2.0')
-import gtk
-import os
-
-import sensor_calibration
-
-class CalibrateGui:
-
-    #
-    # loads a log
-    #
-    def on_load_log(self, widget, data=None):
-        print "Loading log"
-        dialog = gtk.FileChooserDialog("Open..",
-                               None,
-                               gtk.FILE_CHOOSER_ACTION_OPEN,
-                               (gtk.STOCK_CANCEL, gtk.RESPONSE_CANCEL,
-                                gtk.STOCK_OPEN, gtk.RESPONSE_OK))
-        dialog.set_default_response(gtk.RESPONSE_OK)
-        pprz_home = os.environ.get("PAPARAZZI_HOME")
-        if pprz_home != None:
-            dialog.set_current_folder(pprz_home+"/var/logs")
-
-        filter = gtk.FileFilter()
-        filter.set_name("Logs")
-        filter.add_mime_type("paparazzi/logs")
-        filter.add_pattern("*.data")
-        dialog.add_filter(filter)
-
-        filter = gtk.FileFilter()
-        filter.set_name("All files")
-        filter.add_pattern("*")
-        dialog.add_filter(filter)
-        
-        response = dialog.run()
-        if response == gtk.RESPONSE_OK:
-            print dialog.get_filename(), 'selected'
-            ac_ids = sensor_calibration.get_ids_in_log(dialog.get_filename())
-        elif response == gtk.RESPONSE_CANCEL:
-            print 'Closed, no files selected'
-        dialog.destroy()
-        
-    def delete_event(self, widget, event, data=None):
-        print "delete event occurred"
-        return False
-    
-    def destroy(self, widget, data=None):
-        print "destroy signal occurred"
-        gtk.main_quit()
-
-    #
-    # build gui
-    #
-    def build_gui(self):
-        self.window = gtk.Window(gtk.WINDOW_TOPLEVEL)
-        self.window.connect("delete_event", self.delete_event)
-        self.window.connect("destroy", self.destroy)
-        self.window.set_title("Paparazzi Sensor Calibration")
-        self.window.set_border_width(10)
-        table = gtk.Table(2, 2, True)
-        self.window.add(table)
-        table.attach(gtk.Label("Log :"), 0, 1, 0, 1)
-        self.label_log = gtk.Label("None")
-        table.attach(self.label_log, 1, 2, 0, 1)
-        self.button_load_log = gtk.Button("Load log")
-        self.button_load_log.connect("clicked", self.on_load_log, None)
-        table.attach(self.button_load_log, 1, 2, 1, 2)
-        self.window.show_all()   
-
-        
-    def __init__(self):
-        self.build_gui();
-
-    def main(self):
-        gtk.main()
-
-if __name__ == "__main__":
-    app = CalibrateGui()
-    app.main()    

Added: paparazzi3/trunk/sw/tools/calibration/calibrate_gyro.py
===================================================================
--- paparazzi3/trunk/sw/tools/calibration/calibrate_gyro.py                     
        (rev 0)
+++ paparazzi3/trunk/sw/tools/calibration/calibrate_gyro.py     2010-04-29 
20:03:32 UTC (rev 4893)
@@ -0,0 +1,64 @@
+#! /usr/bin/env python
+
+import calibration_utils
+import re
+import scipy
+from scipy import linspace, polyval, polyfit, sqrt, stats, randn
+from pylab import *
+
+
+
+axis_p = 1
+axis_q = 2
+axis_r = 3
+
+
+ac_id = "153"
+tt_id = "43"
+filename = "data/imu_lisa3/log_gyro_q"
+axis = axis_q
+
+#
+# lisa 3
+# p : a=-4511.16 b=31948.34, std error= 0.603
+# q : a=-4598.46 b=31834.48, std error= 0.734
+# r : a=-4525.63 b=32687.95, std error= 0.624
+#
+# lisa 4
+# p : a=-4492.05 b=32684.94, std error= 0.600
+# q : a=-4369.63 b=33260.96, std error= 0.710
+# r : a=-4577.13 b=32707.72, std error= 0.730
+#
+
+samples =  calibration_utils.read_turntable_log(ac_id, tt_id, filename, 1, 7)
+
+
+#Linear regression using stats.linregress
+t  = samples[:,0]
+xn = samples[:,axis]
+(a_s,b_s,r,tt,stderr)=stats.linregress(t,xn)
+print('Linear regression using stats.linregress')
+print('regression: a=%.2f b=%.2f, std error= %.3f' % (a_s,b_s,stderr))
+
+#
+# overlay fited value
+#
+ovl_omega = linspace(1,7.5,10)
+ovl_adc = polyval([a_s,b_s],ovl_omega)
+
+title('Linear Regression Example')
+subplot(3,1,1)
+plot(samples[:,1])
+plot(samples[:,2])
+plot(samples[:,3])
+legend(['p','q','r']);
+
+subplot(3,1,2)
+plot(samples[:,0])
+
+subplot(3,1,3)
+plot(samples[:,0], samples[:,axis], 'b.')
+plot(ovl_omega, ovl_adc, 'r')
+
+show();
+


Property changes on: paparazzi3/trunk/sw/tools/calibration/calibrate_gyro.py
___________________________________________________________________
Added: svn:executable
   + *

Copied: paparazzi3/trunk/sw/tools/calibration/calibration_utils.py (from rev 
4881, paparazzi3/trunk/sw/tools/calibration/sensor_calibration.py)
===================================================================
--- paparazzi3/trunk/sw/tools/calibration/calibration_utils.py                  
        (rev 0)
+++ paparazzi3/trunk/sw/tools/calibration/calibration_utils.py  2010-04-29 
20:03:32 UTC (rev 4893)
@@ -0,0 +1,181 @@
+
+#  $Id$
+#  Copyright (C) 2010 Antoine Drouin
+#
+# This file is part of Paparazzi.
+#
+# Paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# Paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with Paparazzi; see the file COPYING.  If not, write to
+# the Free Software Foundation, 59 Temple Place - Suite 330,
+# Boston, MA 02111-1307, USA.  
+#
+
+import re
+import scipy
+from scipy import linalg
+from pylab import *
+
+
+#
+# returns available ac_id from a log
+#
+def get_ids_in_log(filename):
+    f = open(filename, 'r')
+    ids = []
+    pattern = re.compile("\S+ (\S+)")
+    while 1:
+        line = f.readline().strip()
+        if line == '':
+            break
+        m=re.match(pattern, line)
+        if m:
+            id = m.group(1)
+            if not id in ids:
+              ids.append(id)
+    return ids
+
+#
+# extracts raw sensor measurements from a 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\"/>"
+
+
+
+#
+# plot calibration results
+#
+def plot_results(measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, 
sensor_ref):
+    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')
+    xlabel('time (s)')
+    ylabel('ADC')
+    title('Raw sensors')
+  
+    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();
+
+
+#
+# read a turntable log
+# return an array which first column is turnatble and next 3 are gyro
+#
+def read_turntable_log(ac_id, tt_id, filename, _min, _max):
+    f = open(filename, 'r')
+    pattern_g = re.compile("(\S+) "+ac_id+" IMU_GYRO_RAW (\S+) (\S+) (\S+)")
+    pattern_t = re.compile("(\S+) "+tt_id+" IMU_TURNTABLE (\S+)")
+    last_tt = None
+    list_tt = []
+    while 1:
+        line = f.readline().strip()
+        if line == '':
+            break
+        m=re.match(pattern_t, line)
+        if m:
+            last_tt = float(m.group(2))
+        m=re.match(pattern_g, line)
+        if m and last_tt and last_tt > _min and last_tt < _max:
+            list_tt.append([last_tt, float(m.group(2)), float(m.group(3)), 
float(m.group(4))])
+    return scipy.array(list_tt)           
+
+#
+#
+#

Deleted: paparazzi3/trunk/sw/tools/calibration/sensor_calibration.py
===================================================================
--- paparazzi3/trunk/sw/tools/calibration/sensor_calibration.py 2010-04-29 
12:31:21 UTC (rev 4892)
+++ paparazzi3/trunk/sw/tools/calibration/sensor_calibration.py 2010-04-29 
20:03:32 UTC (rev 4893)
@@ -1,154 +0,0 @@
-
-#  $Id$
-#  Copyright (C) 2010 Antoine Drouin
-#
-# This file is part of Paparazzi.
-#
-# Paparazzi is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2, or (at your option)
-# any later version.
-#
-# Paparazzi is distributed in the hope that it will be useful,
-# but WITHOUT ANY WARRANTY; without even the implied warranty of
-# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-# GNU General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with Paparazzi; see the file COPYING.  If not, write to
-# the Free Software Foundation, 59 Temple Place - Suite 330,
-# Boston, MA 02111-1307, USA.  
-#
-
-import re
-import scipy
-from scipy import linalg
-from pylab import *
-
-
-#
-# returns available ac_id from a log
-#
-def get_ids_in_log(filename):
-    f = open(filename, 'r')
-    ids = []
-    pattern = re.compile("\S+ (\S+)")
-    while 1:
-        line = f.readline().strip()
-        if line == '':
-            break
-        m=re.match(pattern, line)
-        if m:
-            id = m.group(1)
-            if not id in ids:
-              ids.append(id)
-    return ids
-
-#
-# extracts raw sensor measurements from a 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\"/>"
-
-
-
-#
-# plot calibration results
-#
-def plot_results(measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, 
sensor_ref):
-    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')
-    xlabel('time (s)')
-    ylabel('ADC')
-    title('Raw sensors')
-  
-    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();