mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-26 08:22:43 +08:00
toying around with calibration
This commit is contained in:
Antoine Drouin
@@ -28,7 +28,7 @@ from optparse import OptionParser
|
||||
import scipy
|
||||
from scipy import optimize
|
||||
|
||||
import sensor_calibration
|
||||
import calibration_utils
|
||||
|
||||
|
||||
def main():
|
||||
@@ -51,13 +51,13 @@ def main():
|
||||
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 @@ def main():
|
||||
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()
|
||||
|
||||
@@ -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()
|
||||
Executable
+64
@@ -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();
|
||||
|
||||
+27
@@ -152,3 +152,30 @@ def plot_results(measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref
|
||||
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)
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
Reference in New Issue
Block a user