find best magnetometer offset fit to a log file

def magfit(logfile):
    '''find best magnetometer offset fit to a log file'''

    print("Processing log %s" % filename)
    mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)

    data = []

    last_t = 0
    offsets = Vector3(0,0,0)

    # now gather all the data
    while True:
        m = mlog.recv_match(condition=args.condition)
        if m is None:
            break
        if m.get_type() == "SENSOR_OFFSETS":
            # update current offsets
            offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
        if m.get_type() == "RAW_IMU":
            mag = Vector3(m.xmag, m.ymag, m.zmag)
            # add data point after subtracting the current offsets
            data.append(mag - offsets + noise())
        if m.get_type() == "MAG" and not args.mag2:
            offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
            mag = Vector3(m.MagX,m.MagY,m.MagZ)
            data.append(mag - offsets + noise())
        if m.get_type() == "MAG2" and args.mag2:
            offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
            mag = Vector3(m.MagX,m.MagY,m.MagZ)
            data.append(mag - offsets + noise())

    print("Extracted %u data points" % len(data))
    print("Current offsets: %s" % offsets)

    orig_data = data

    data = select_data(data)

    # remove initial outliers
    data.sort(lambda a,b : radius_cmp(a,b,offsets))
    data = data[len(data)/16:-len(data)/16]

    # do an initial fit
    (offsets, field_strength) = fit_data(data)

    for count in range(3):
        # sort the data by the radius
        data.sort(lambda a,b : radius_cmp(a,b,offsets))

        print("Fit %u    : %s  field_strength=%6.1f to %6.1f" % (
            count, offsets,
            radius(data[0], offsets), radius(data[-1], offsets)))

        # discard outliers, keep the middle 3/4
        data = data[len(data)/8:-len(data)/8]

        # fit again
        (offsets, field_strength) = fit_data(data)

    print("Final    : %s  field_strength=%6.1f to %6.1f" % (
        offsets,
        radius(data[0], offsets), radius(data[-1], offsets)))

    if args.plot:
        plot_data(orig_data, data)