magfit_elliptical.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 
8 import sys, time, os, math
9 
10 from argparse import ArgumentParser
11 parser = ArgumentParser(description=__doc__)
12 parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
13 parser.add_argument("--condition", default=None, help="select packets by condition")
14 parser.add_argument("--noise", type=float, default=0, help="noise to add")
15 parser.add_argument("--mag2", action='store_true', help="use 2nd mag from DF log")
16 parser.add_argument("--radius", default=500.0, type=float, help="target radius")
17 parser.add_argument("--plot", action='store_true', help="plot points in 3D")
18 parser.add_argument("logs", metavar="LOG", nargs="+")
19 
20 args = parser.parse_args()
21 
22 from pymavlink import mavutil
23 from pymavlink.rotmat import Vector3
24 from pymavlink.rotmat import Matrix3
25 
26 
27 def noise():
28  '''a noise vector'''
29  from random import gauss
30  v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
31  v.normalize()
32  return v * args.noise
33 
34 def select_data(data):
35  ret = []
36  counts = {}
37  for d in data:
38  mag = d
39  key = "%u:%u:%u" % (mag.x/20,mag.y/20,mag.z/20)
40  if key in counts:
41  counts[key] += 1
42  else:
43  counts[key] = 1
44  if counts[key] < 3:
45  ret.append(d)
46  print((len(data), len(ret)))
47  return ret
48 
49 def constrain(v, min, max):
50  if v < min:
51  return min
52  if v > max:
53  return max
54  return v
55 
56 def correct(mag, offsets, diag, offdiag):
57  '''correct a mag sample'''
58  diag.x = 1.0
59  mat = Matrix3(
60  Vector3(diag.x, offdiag.x, offdiag.y),
61  Vector3(offdiag.x, diag.y, offdiag.z),
62  Vector3(offdiag.y, offdiag.z, diag.z))
63  mag = mag + offsets
64  mag = mat * mag
65  return mag
66 
67 def radius(mag, offsets, diag, offdiag):
68  '''return radius give data point and offsets'''
69  mag = correct(mag, offsets, diag, offdiag)
70  return mag.length()
71 
72 def radius_cmp(a, b, offsets, diag, offdiag):
73  '''return +1 or -1 for for sorting'''
74  diff = radius(a, offsets, diag, offdiag) - radius(b, offsets, diag, offdiag)
75  if diff > 0:
76  return 1
77  if diff < 0:
78  return -1
79  return 0
80 
81 def sphere_error(p, data):
82  x,y,z,r,dx,dy,dz,odx,ody,odz = p
83  if args.radius is not None:
84  r = args.radius
85  ofs = Vector3(x,y,z)
86  diag = Vector3(dx, dy, dz)
87  offdiag = Vector3(odx, ody, odz)
88 
89  ret = []
90  for d in data:
91  mag = correct(d, ofs, diag, offdiag)
92  err = r - mag.length()
93  ret.append(err)
94  return ret
95 
96 def fit_data(data):
97  from scipy import optimize
98 
99  p0 = [0.0, 0.0, 0.0, # offsets
100  500.0, # radius
101  1.0, 1.0, 1.0, # diagonals
102  0.0, 0.0, 0.0 # offdiagonals
103  ]
104  if args.radius is not None:
105  p0[3] = args.radius
106  p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data))
107  if not ier in [1, 2, 3, 4]:
108  print(p1)
109  raise RuntimeError("Unable to find solution: %u" % ier)
110  if args.radius is not None:
111  r = args.radius
112  else:
113  r = p1[3]
114  return (Vector3(p1[0], p1[1], p1[2]),
115  r,
116  Vector3(p1[4], p1[5], p1[6]),
117  Vector3(p1[7], p1[8], p1[9]))
118 
119 def magfit(logfile):
120  '''find best magnetometer offset fit to a log file'''
121 
122  print("Processing log %s" % filename)
123  mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
124 
125  data = []
126 
127  last_t = 0
128  offsets = None
129 
130  # now gather all the data
131  while True:
132  m = mlog.recv_match(condition=args.condition)
133  if m is None:
134  break
135  if m.get_type() == "SENSOR_OFFSETS":
136  # update current offsets
137  offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
138  if m.get_type() == "RAW_IMU":
139  mag = Vector3(m.xmag, m.ymag, m.zmag)
140  # add data point after subtracting the current offsets
141  if offsets is not None:
142  data.append(mag - offsets + noise())
143  if m.get_type() == "MAG" and not args.mag2:
144  offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
145  mag = Vector3(m.MagX,m.MagY,m.MagZ)
146  data.append(mag - offsets + noise())
147  if m.get_type() == "MAG2" and args.mag2:
148  offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
149  mag = Vector3(m.MagX,m.MagY,m.MagZ)
150  data.append(mag - offsets + noise())
151 
152  print("Extracted %u data points" % len(data))
153  print("Current offsets: %s" % offsets)
154 
155  orig_data = data
156 
157  # find average values
158  avg = Vector3()
159  count = 0
160  for d in data:
161  avg += d
162  count += 1
163  avg /= count
164 
165  # subtract average
166  data = []
167  for d in orig_data:
168  data.append(d - avg)
169  print("Average %s" % avg)
170 
171  #data = select_data(data)
172 
173  diag = Vector3(1,1,1)
174  offdiag = Vector3(0,0,0)
175 
176  # remove initial outliers
177  if False:
178  data.sort(lambda a,b : radius_cmp(a,b,offsets,diag,offdiag))
179  data = data[len(data)/16:-len(data)/16]
180 
181  # do an initial fit
182  (offsets, field_strength, diag, offdiag) = fit_data(data)
183 
184  for count in range(3):
185  # sort the data by the radius
186  data.sort(lambda a,b : radius_cmp(a,b,offsets,diag,offdiag))
187 
188  print("Fit %u : %s %s %s field_strength=%6.1f to %6.1f" % (
189  count, offsets, diag, offdiag,
190  radius(data[0], offsets,diag,offdiag), radius(data[-1], offsets,diag,offdiag)))
191 
192  # discard outliers, keep the middle
193  data = data[len(data)/32:-len(data)/32]
194 
195  # fit again
196  (offsets, field_strength, diag, offdiag) = fit_data(data)
197 
198  print("Final : %s %s %s field_strength=%6.1f to %6.1f" % (
199  offsets, diag, offdiag,
200  radius(data[0], offsets, diag, offdiag), radius(data[-1], offsets, diag, offdiag)))
201 
202  offsets -= avg
203  print("With average : %s" % offsets)
204 
205  if args.plot:
206  data2 = [correct(d,offsets,diag,offdiag) for d in orig_data]
207  plot_data(orig_data, data2)
208 
209 def plot_data(orig_data, data):
210  '''plot data in 3D'''
211  from mpl_toolkits.mplot3d import Axes3D
212  import matplotlib.pyplot as plt
213 
214  fig = plt.figure()
215  for dd, c, p in [(orig_data, 'r', 1), (data, 'b', 2)]:
216  ax = fig.add_subplot(1, 2, p, projection='3d')
217 
218  xs = [ d.x for d in dd ]
219  ys = [ d.y for d in dd ]
220  zs = [ d.z for d in dd ]
221  ax.scatter(xs, ys, zs, c=c, marker='o')
222 
223  ax.set_xlabel('X')
224  ax.set_ylabel('Y')
225  ax.set_zlabel('Z')
226  plt.show()
227 
228 total = 0.0
229 for filename in args.logs:
230  magfit(filename)


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