import sysimport numpy as npfrom scipy import linalgclass Magnetometer(object):    ''' Magnetometer class with calibration capabilities.        Parameters        ----------        sensor : str            Sensor to use.        bus : int            Bus where the sensor is attached.        F : float (optional)            Expected earth magnetic field intensity, default=1.    '''    # available sensors    _sensors = {'ak8975c': SensorAK8975C}    def __init__(self, sensor, bus, F=1.):        # sensor        if sensor not in self._sensors:            raise NotImplementedError('Sensor %s not available' % sensor)        self.sensor = self._sensors[sensor](bus)        # initialize values        self.F   = F        self.b   = np.zeros([3, 1])        self.A_1 = np.eye(3)    def read(self):        ''' Get a sample.            Returns            -------            s : list                The sample in uT, [x,y,z] (corrected if performed calibration).        '''        s = np.array(self.sensor.read()).reshape(3, 1)        s = np.dot(self.A_1, s - self.b)        return [s[0,0], s[1,0], s[2,0]]    def calibrate(self):        ''' Performs calibration. '''        print('Collecting samples (Ctrl-C to stop and perform calibration)')        try:            s = []            n = 0            while True:                s.append(self.sensor.read())                n += 1                sys.stdout.write('\rTotal: %d' % n)                sys.stdout.flush()        except KeyboardInterrupt:            pass        # ellipsoid fit        s = np.array(s).T        M, n, d = self.__ellipsoid_fit(s)        # calibration parameters        # note: some implementations of sqrtm return complex type, taking real        M_1 = linalg.inv(M)        self.b = -np.dot(M_1, n)        self.A_1 = np.real(self.F / np.sqrt(np.dot(n.T, np.dot(M_1, n)) - d) *                           linalg.sqrtm(M))    def __ellipsoid_fit(self, s):        ''' Estimate ellipsoid parameters from a set of points.            Parameters            ----------            s : array_like              The samples (M,N) where M=3 (x,y,z) and N=number of samples.            Returns            -------            M, n, d : array_like, array_like, float              The ellipsoid parameters M, n, d.            References            ----------            .. [1] Qingde Li; Griffiths, J.G., "Least squares ellipsoid specific               fitting," in Geometric Modeling and Processing, 2004.               Proceedings, vol., no., pp.335-340, 2004        '''        # D (samples)        D = np.array([s[0]**2., s[1]**2., s[2]**2.,                      2.*s[1]*s[2], 2.*s[0]*s[2], 2.*s[0]*s[1],                      2.*s[0], 2.*s[1], 2.*s[2], np.ones_like(s[0])])        # S, S_11, S_12, S_21, S_22 (eq. 11)        S = np.dot(D, D.T)        S_11 = S[:6,:6]        S_12 = S[:6,6:]        S_21 = S[6:,:6]        S_22 = S[6:,6:]        # C (Eq. 8, k=4)        C = np.array([[-1,  1,  1,  0,  0,  0],                      [ 1, -1,  1,  0,  0,  0],                      [ 1,  1, -1,  0,  0,  0],                      [ 0,  0,  0, -4,  0,  0],                      [ 0,  0,  0,  0, -4,  0],                      [ 0,  0,  0,  0,  0, -4]])        # v_1 (eq. 15, solution)        E = np.dot(linalg.inv(C),                   S_11 - np.dot(S_12, np.dot(linalg.inv(S_22), S_21)))        E_w, E_v = np.linalg.eig(E)        v_1 = E_v[:, np.argmax(E_w)]        if v_1[0] < 0: v_1 = -v_1        # v_2 (eq. 13, solution)        v_2 = np.dot(np.dot(-np.linalg.inv(S_22), S_21), v_1)        # quadric-form parameters        M = np.array([[v_1[0], v_1[3], v_1[4]],                      [v_1[3], v_1[1], v_1[5]],                      [v_1[4], v_1[5], v_1[2]]])        n = np.array([[v_2[0]],                      [v_2[1]],                      [v_2[2]]])        d = v_2[3]

return M, n, d

Example

To conclude this post we will use samples from a real magnetometer, which willallow us to see if the proposed solution behaves as expected. It is important tonote that we will not evaluate the accuracy of our solution, neither otherthings such as how the number of samples or their spatial distribution affectsthe calibration. These are topics that would require further study, out of thescope of this post.

The chosen magnetometer is the one found inside the InvensenseMPU-9150: AK8975C. It is a module thatalso contains an accelerometer and a gyroscope, not required for our purposes. ARaspberry Pi 2 will be used as a host platform, as shown below. This will allowus to quickly code a proof of concept by just using a few lines of Python. Notethat you will need to activate the I2C driver and install python-smbus on theRaspberry Pi (detailshere).

The proposed code for the AK8975C drive is:

import smbus# MPU9150 used registersMPU9150_ADDR                       = 0x68MPU9150_PWR_MGMT_1                 = 0x6BMPU9150_INT_PIN_CFG                = 0x37MPU9150_INT_PIN_CFG_I2C_BYPASS_EN  = 0x02# AK8975C used registersAK8975C_ADDR                       = 0x0CAK8975C_ST1                        = 0x02AK8975C_ST1_DRDY                   = 0x01AK8975C_HXL                        = 0x03AK8975C_CNTL                       = 0x0AAK8975C_CNTL_SINGLE                = 0x01# convert to s16 from two's complementto_s16 = lambda n: n - 0x10000 if n > 0x7FFF else nclass SensorAK8975C(object):    ''' AK8975C Simple Driver.        Parameters        ----------        bus : int            I2C bus number (e.g. 1).    '''    # sensor sensitivity (uT/LSB)    _sensitivity = 0.3    def __init__(self, bus):        self.bus = smbus.SMBus(bus)        # wake up and set bypass for AK8975C        self.bus.write_byte_data(MPU9150_ADDR, MPU9150_PWR_MGMT_1, 0)        self.bus.write_byte_data(MPU9150_ADDR, MPU9150_INT_PIN_CFG,                                 MPU9150_INT_PIN_CFG_I2C_BYPASS_EN)    def __del__(self):        self.bus.close()    def read(self):        ''' Get a sample.            Returns            -------            s : list                The sample in uT, [x, y, z].        '''        # request single shot        self.bus.write_byte_data(AK8975C_ADDR, AK8975C_CNTL,                                               AK8975C_CNTL_SINGLE)        # wait for data ready        r = self.bus.read_byte_data(AK8975C_ADDR, AK8975C_ST1)        while not (r & AK8975C_ST1_DRDY):            r = self.bus.read_byte_data(AK8975C_ADDR, AK8975C_ST1)        # read x, y, z        data = self.bus.read_i2c_block_data(AK8975C_ADDR, AK8975C_HXL, 6)        return [self._sensitivity * to_s16(data[0] | (data[1] << 8)),                self._sensitivity * to_s16(data[2] | (data[3] << 8)),                self._sensitivity * to_s16(data[4] | (data[5] << 8))]

Merging the sensor driver shown above with the code in the previous section willallow us to both sample and calibrate. We can also append the code shown below,which will allow us to store non-calibrated and calibrated samples to thenvisualize the results with any plotting library:

from time import sleepdef collect(fn, fs=10):    ''' Collect magnetometer samples        Parameters        ----------        fn : str            Output file.        fs : int (optional)            Approximate sampling frequency (Hz), default 10 Hz.    '''    print('Collecting [%s]. Ctrl-C to finish' % fn)    with open(fn, 'w') as f:        f.write('x,y,z\n')        try:            while True:                s = m.read()                f.write('%.1f,%.1f,%.1f\n' % (s[0], s[1], s[2]))                sleep(1./fs)        except KeyboardInterrupt: passm = Magnetometer('ak8975c', 1, 46.85)collect('ncal.csv')m.calibrate()