magfit.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 fit best estimate of magnetometer offsets
00005 '''
00006 
00007 import sys, time, os, math
00008 
00009 from argparse import ArgumentParser
00010 parser = ArgumentParser(description=__doc__)
00011 parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
00012 parser.add_argument("--condition", default=None, help="select packets by condition")
00013 parser.add_argument("--noise", type=float, default=0, help="noise to add")
00014 parser.add_argument("--mag2", action='store_true', help="use 2nd mag from DF log")
00015 parser.add_argument("logs", metavar="LOG", nargs="+")
00016 
00017 args = parser.parse_args()
00018 
00019 from pymavlink import mavutil
00020 from pymavlink.rotmat import Vector3
00021 
00022 
00023 def noise():
00024     '''a noise vector'''
00025     from random import gauss
00026     v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
00027     v.normalize()
00028     return v * args.noise
00029 
00030 def select_data(data):
00031     ret = []
00032     counts = {}
00033     for d in data:
00034         mag = d
00035         key = "%u:%u:%u" % (mag.x/20,mag.y/20,mag.z/20)
00036         if key in counts:
00037             counts[key] += 1
00038         else:
00039             counts[key] = 1
00040         if counts[key] < 3:
00041             ret.append(d)
00042     print(len(data), len(ret))
00043     return ret
00044 
00045 def radius(mag, offsets):
00046     '''return radius give data point and offsets'''
00047     return (mag + offsets).length()
00048 
00049 def radius_cmp(a, b, offsets):
00050     '''return radius give data point and offsets'''
00051     diff = radius(a, offsets) - radius(b, offsets)
00052     if diff > 0:
00053         return 1
00054     if diff < 0:
00055         return -1
00056     return 0
00057 
00058 def sphere_error(p, data):
00059     from scipy import sqrt
00060     x,y,z,r = p
00061     ofs = Vector3(x,y,z)
00062     ret = []
00063     for d in data:
00064         mag = d
00065         err = r - radius(mag, ofs)
00066         ret.append(err)
00067     return ret
00068 
00069 def fit_data(data):
00070     import numpy, scipy
00071     from scipy import optimize
00072 
00073     p0 = [0.0, 0.0, 0.0, 0.0]
00074     p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data))
00075     if not ier in [1, 2, 3, 4]:
00076         raise RuntimeError("Unable to find solution")
00077     return (Vector3(p1[0], p1[1], p1[2]), p1[3])
00078 
00079 def magfit(logfile):
00080     '''find best magnetometer offset fit to a log file'''
00081 
00082     print("Processing log %s" % filename)
00083     mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
00084 
00085     data = []
00086 
00087     last_t = 0
00088     offsets = Vector3(0,0,0)
00089 
00090     # now gather all the data
00091     while True:
00092         m = mlog.recv_match(condition=args.condition)
00093         if m is None:
00094             break
00095         if m.get_type() == "SENSOR_OFFSETS":
00096             # update current offsets
00097             offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
00098         if m.get_type() == "RAW_IMU":
00099             mag = Vector3(m.xmag, m.ymag, m.zmag)
00100             # add data point after subtracting the current offsets
00101             data.append(mag - offsets + noise())
00102         if m.get_type() == "MAG" and not args.mag2:
00103             offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
00104             mag = Vector3(m.MagX,m.MagY,m.MagZ)
00105             data.append(mag - offsets + noise())
00106         if m.get_type() == "MAG2" and args.mag2:
00107             offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
00108             mag = Vector3(m.MagX,m.MagY,m.MagZ)
00109             data.append(mag - offsets + noise())
00110 
00111     print("Extracted %u data points" % len(data))
00112     print("Current offsets: %s" % offsets)
00113 
00114     data = select_data(data)
00115 
00116     # remove initial outliers
00117     data.sort(lambda a,b : radius_cmp(a,b,offsets))
00118     data = data[len(data)/16:-len(data)/16]
00119 
00120     # do an initial fit
00121     (offsets, field_strength) = fit_data(data)
00122 
00123     for count in range(3):
00124         # sort the data by the radius
00125         data.sort(lambda a,b : radius_cmp(a,b,offsets))
00126 
00127         print("Fit %u    : %s  field_strength=%6.1f to %6.1f" % (
00128             count, offsets,
00129             radius(data[0], offsets), radius(data[-1], offsets)))
00130 
00131         # discard outliers, keep the middle 3/4
00132         data = data[len(data)/8:-len(data)/8]
00133 
00134         # fit again
00135         (offsets, field_strength) = fit_data(data)
00136 
00137     print("Final    : %s  field_strength=%6.1f to %6.1f" % (
00138         offsets,
00139         radius(data[0], offsets), radius(data[-1], offsets)))
00140 
00141 total = 0.0
00142 for filename in args.logs:
00143     magfit(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43