magfit.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 fit best estimate of magnetometer offsets
5 '''
6 from __future__ import print_function
7 from builtins import range
8 
9 from argparse import ArgumentParser
10 parser = ArgumentParser(description=__doc__)
11 parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
12 parser.add_argument("--condition", default=None, help="select packets by condition")
13 parser.add_argument("--noise", type=float, default=0, help="noise to add")
14 parser.add_argument("--mag2", action='store_true', help="use 2nd mag from DF log")
15 parser.add_argument("--radius", default=None, type=float, help="target radius")
16 parser.add_argument("--plot", action='store_true', help="plot points in 3D")
17 parser.add_argument("logs", metavar="LOG", nargs="+")
18 
19 args = parser.parse_args()
20 
21 from pymavlink import mavutil
22 from pymavlink.rotmat import Vector3
23 
24 
25 def noise():
26  '''a noise vector'''
27  from random import gauss
28  v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
29  v.normalize()
30  return v * args.noise
31 
32 def select_data(data):
33  ret = []
34  counts = {}
35  for d in data:
36  mag = d
37  key = "%u:%u:%u" % (mag.x/20,mag.y/20,mag.z/20)
38  if key in counts:
39  counts[key] += 1
40  else:
41  counts[key] = 1
42  if counts[key] < 3:
43  ret.append(d)
44  print(len(data), len(ret))
45  return ret
46 
47 def radius(mag, offsets):
48  '''return radius give data point and offsets'''
49  return (mag + offsets).length()
50 
51 def radius_cmp(a, b, offsets):
52  '''return +1 or -1 for for sorting'''
53  diff = radius(a, offsets) - radius(b, offsets)
54  if diff > 0:
55  return 1
56  if diff < 0:
57  return -1
58  return 0
59 
60 def sphere_error(p, data):
61  x,y,z,r = p
62  if args.radius is not None:
63  r = args.radius
64  ofs = Vector3(x,y,z)
65  ret = []
66  for d in data:
67  mag = d
68  err = r - radius(mag, ofs)
69  ret.append(err)
70  return ret
71 
72 def fit_data(data):
73  from scipy import optimize
74 
75  p0 = [0.0, 0.0, 0.0, 0.0]
76  p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data))
77  if not ier in [1, 2, 3, 4]:
78  raise RuntimeError("Unable to find solution")
79  if args.radius is not None:
80  r = args.radius
81  else:
82  r = p1[3]
83  return (Vector3(p1[0], p1[1], p1[2]), r)
84 
85 def magfit(logfile):
86  '''find best magnetometer offset fit to a log file'''
87 
88  print("Processing log %s" % filename)
89  mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
90 
91  data = []
92 
93  last_t = 0
94  offsets = Vector3(0,0,0)
95 
96  # now gather all the data
97  while True:
98  m = mlog.recv_match(condition=args.condition)
99  if m is None:
100  break
101  if m.get_type() == "SENSOR_OFFSETS":
102  # update current offsets
103  offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
104  if m.get_type() == "RAW_IMU":
105  mag = Vector3(m.xmag, m.ymag, m.zmag)
106  # add data point after subtracting the current offsets
107  data.append(mag - offsets + noise())
108  if m.get_type() == "MAG" and not args.mag2:
109  offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
110  mag = Vector3(m.MagX,m.MagY,m.MagZ)
111  data.append(mag - offsets + noise())
112  if m.get_type() == "MAG2" and args.mag2:
113  offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
114  mag = Vector3(m.MagX,m.MagY,m.MagZ)
115  data.append(mag - offsets + noise())
116 
117  print("Extracted %u data points" % len(data))
118  print("Current offsets: %s" % offsets)
119 
120  orig_data = data
121 
122  data = select_data(data)
123 
124  # remove initial outliers
125  data.sort(lambda a,b : radius_cmp(a,b,offsets))
126  data = data[len(data)/16:-len(data)/16]
127 
128  # do an initial fit
129  (offsets, field_strength) = fit_data(data)
130 
131  for count in range(3):
132  # sort the data by the radius
133  data.sort(lambda a,b : radius_cmp(a,b,offsets))
134 
135  print("Fit %u : %s field_strength=%6.1f to %6.1f" % (
136  count, offsets,
137  radius(data[0], offsets), radius(data[-1], offsets)))
138 
139  # discard outliers, keep the middle 3/4
140  data = data[len(data)/8:-len(data)/8]
141 
142  # fit again
143  (offsets, field_strength) = fit_data(data)
144 
145  print("Final : %s field_strength=%6.1f to %6.1f" % (
146  offsets,
147  radius(data[0], offsets), radius(data[-1], offsets)))
148 
149  if args.plot:
150  plot_data(orig_data, data)
151 
152 def plot_data(orig_data, data):
153  '''plot data in 3D'''
154  import matplotlib.pyplot as plt
155 
156  for dd, c in [(orig_data, 'r'), (data, 'b')]:
157  fig = plt.figure()
158  ax = fig.add_subplot(111, projection='3d')
159 
160  xs = [ d.x for d in dd ]
161  ys = [ d.y for d in dd ]
162  zs = [ d.z for d in dd ]
163  ax.scatter(xs, ys, zs, c=c, marker='o')
164 
165  ax.set_xlabel('X Label')
166  ax.set_ylabel('Y Label')
167  ax.set_zlabel('Z Label')
168  plt.show()
169 
170 total = 0.0
171 for filename in args.logs:
172  magfit(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:06