Monthly Archives: June 2016

Giving Wall-E2 A Sense of Direction, Part VI

Posted 06/28/16

In my last post on this subject, I used my new Magnetometer to generate calibration matrix/center offset values for my Mongoose IMU (which uses a HMC5883 3-axis magnetometer), in a ‘free space’ (no nearby magnetic interferers) environment, and showed that I could incorporate these values into the Mongoose’s firmware.  In this post, I describe my efforts to calibrate the same Mongoose IMU, but now mounted on Wall-E2, my 4WD wall-following robot.


Mongoose IMU (see arrow) mounted on front of Wall-E2

A long time ago, in a galaxy far, far away (actually 3 months ago, in the exact same galaxy), I had the Mongoose IMU mounted on the front of my robot, as shown in the above image.  Unfortunately, when I tried to use the heading data from the Mongoose (see Giving Wall-E2 a Sense of Direction, Part IV), it was readily apparent that something was badly wrong.  Eventually I figured out that the problem was the magnetic fields associated with the drive motors that were causing the problem, and I wouldn’t be able to do much about that without some sort of calibration exercise.  After this realization I tried, unsuccessfully, to find a magnetometer calibration tool that I liked.  Failing that, I wrote my own (twice!), winding up with the WPF-based application described in ‘Magnetometer Calibration, Part III‘.

So, now the idea is to re-mount the Mongoose IMU on Wall-E2, and use my newly-created calibration tool to compensate for the magnetic interference generated by the DC motors and operating currents.  As a first step in that direction, I decided to mount the IMU on a wooden stalk on the top of the robot, thereby gaining as much separation from the motors and other interferers as possible.  If this works, then I will try to reduce the height of the stalk as much as possible.

The image below shows the initial mounting setup.

Mongoose IMU mounted on wood stalk

Mongoose IMU mounted on wood stalk

With the Mongoose mounted as shown, I used my magnetometer calibration tool to generate a calibration matrix and center offset, as shown in the following image.

Calibration run for Mongoose IMU mounted on wood stalk on top of Wall-E2

Calibration run for Mongoose IMU mounted on wood stalk on top of Wall-E2




Giving Wall-E2 A Sense of Direction, Part V

Posted 06/18/16

My last few posts have described my efforts to create an easy-to-use magnetometer calibration utility to allow for as-installed magnetometer calibration.  In situ calibration is necessary for magnetometers because they can be significantly affected by nearby ‘hard’ and ‘soft’ iron interferers.  In my research on this topic, I discovered there were two main magnetometer calibration methods; in one, 3-axis magnetometer data is acquired with the entire assembly containing the magnetometer placed in a small but complete number of well-known positions.  The data is then manipulated to generate calibration values that are then used to convert magnetometer data at an arbitrary position.  The other method involves  acquiring a large amount (hundreds or thousands of points) of data while the assembly is rotated arbitrarily around all three axes.  The compensation method assumes the acquired data is sufficiently varied to cover the entire 3D sphere, and then finds the best fit of the data to a perfect sphere centered at the origin.  This produces an upper triangular 3×3 matrix of multiplicative values and an offset vector that can be used to convert any magnetometer position raw value to a compensated one.  I decided to create a tool using the second method, mainly because I had available a MATLAB script that would do most of the work for me, and Octave, the free open-source application that can execute most MATLAB scripts.  Moreover, Octave for windows can be called from C#/.NET programs, making it a natural fit for my needs.  In any case, I was able to implement the utility (twice!!) over the course of a couple of months, getting it to the point where I am now ready to try calibrating my CK Devices ‘Mongoose’ IMU, as installed on my ‘Wall-E2’ four-wheel drive robot.

However, before mounting the IMU on the robot and going for ‘the big Kahuna’ result, I decided to essentially re-create my original experiment with the IMU rotated in the X-Y plane on my bench-top, as described in the post ‘Giving Wall-E2 A Sense of Direction – Part III‘.  My 4-inch compass rose had long since bitten the dust, but I had saved the print file (did I tell you that I never throw anything away)

Mongoose IMU on 4-Inch Compass Rose

Mongoose IMU on 4-Inch Compass Rose

So, I basically re-created the original heading error test from back in March, and got similar (but not identical) results, as shown below:

Heading Error, Compensation, and Comp+Error

Heading Error, Compensation, and Comp+Error

06/19/16 Mongoose 'Desktop' Heading Error

06/19/16 Mongoose ‘Desktop’ Heading Error

Then I used my newly minted magnetometer calibration utility to generate a calibration matrix and center offset, so I can apply them to the above data.  However, before I can do that I have to go back into CK Devices original code to find out where the calibration should be applied – more digging :-(.

In the original Mongoose IMU code, the function ‘ReadCompass()’ in HMC5883L.ino gets the raw values from the magnetometer and generates compensated values using whatever values the user places in two ‘struct’ objects (all zeros by default).  However, I was clever enough to only send the ‘raw’ uncalibrated magnetometer data to the serial port, so that is what I’ve been using as ‘raw’ data for my mag calibration tool – so far, so good.  However, what I need for my robot is compensated values, so (hopefully) I can (accurately?) determine Wall-E2’s heading.

So, it appears I have two options here; I can continue to emit ‘raw’ data from the Mongoose and perform any needed compensation externally, or I can do the compensation internally to the Mongoose and emit only corrected mag data.  The problem with the latter option (internal to the Mongoose) is that I would have to defeat it each time the robot configuration changed, with it’s inevitable change to the magnetometer’s surroundings.  If I write an external routine to do the compensation based on the results from the calibration tool, then it is only that one routine that will require an update.  OTOH, If the compensation is internal to the Mongoose, then modularity is maximized – a very good feature.  The deciding factor is that if the routine is internal to the Moongoose, then I can remove it from the robot and I still have a complete setup for magnetometer work.  So, I decided to write it into the Mongoose code,  but have the ability to switch it in/out with a compile time switch (something like NO_MAGCOMP?)

The compensation expression being implemented is:

W = U*(V-C), where U = spherical compensation matrix, V = raw mag values, C = center offset value

Since U is always upper triangular (don’t ask – I don’t know why), the above matrix expression simplifies to:

Wx = U11*(Vx-Cx) + U12*(Vy-Cy) + U13*(Vz-Cz)
Wy = U22*(Vy-Cy) + U23*(Vz-Cz)
Wz = U33*(Vz-Cz)

I implemented the above expression in the Mongoose firmware by adding a new function ‘CalibrateMagData()’ as follows:

void CalibrateMagData()
	//Purpose:  Apply spherical compensation expression (vals from mag cal tool)
	//Expression is W = U*(V-C), where U is u.t. comp matrix, V is raw data, C is ctr offset

	float raw_x = sen_data.magnetom_x_raw;
	float raw_y = sen_data.magnetom_y_raw;
	float raw_z = sen_data.magnetom_z_raw;

	sen_data.magnetom_x = magcalvals.U11*(raw_x - magcalvals.Cx)
		+ magcalvals.U12*(raw_y - magcalvals.Cy)
		+ magcalvals.U13*(raw_z - magcalvals.Cz);

	sen_data.magnetom_y = magcalvals.U22*(raw_y - magcalvals.Cy)
		+ magcalvals.U23*(raw_z - magcalvals.Cz);

	sen_data.magnetom_z = magcalvals.U33*(raw_z - magcalvals.Cz);


Using the already existing s_sensor_data struct which is defined as follows:

struct s_sensor_data
    //raw data is uncorrected and corresponds to the
    //true sensor axis, not the redefined platform orientation
    int gyro_x_raw;
    int gyro_y_raw;
    int gyro_z_raw;
    int accel_x_raw;
    int accel_y_raw;
    int accel_z_raw;
    int magnetom_x_raw;
    int magnetom_y_raw;
    int magnetom_z_raw;
    //This data has been corrected based on the calibration values
    float gyro_x;
    float gyro_y;
    float gyro_z;
    float accel_x;
    float accel_y;
    float accel_z;
    float magnetom_x;
    float magnetom_y;
    float magnetom_z;
    float magnetom_heading;
    short baro_temp;  
    long baro_pres;

Then I created another ‘print’ routine, ‘PrintMagCalData()’ to print out the calibrated (vs raw) magnetometer data. Also, after an overnight dream-state ‘aha’ moment, I realized I don’t have to incorporate a compile-time #ifdef statement to switch between ‘raw’ and ‘calibrated’ data readout from the Mongoose – I simply attach a jumper from either GND or +3.3V to one of the I/O pins, and implement code that calls either ‘PrintMagCalData()’ or ‘PrintMagRawData()’ depending on the HIGH/LOW state of the monitor pin. Now that’s elegant! 😉

After making these changes, I fired up just the Mongoose using VS2015 in debug mode, which includes a port monitor function.  As soon as the Moongoose came up, it started spitting out 3D magnetometer data – YAY!!

It’s been a few days since I got this going – my wife and I went off to a weekend bridge tournament in Kentucky and we got back late last night – so I didn’t get a chance to compare the ‘after-calibration’ heading performance with the ‘before’ version until today.

After Calibration Magnetic Heading Error

After Calibration Magnetic Heading Error

Comparing the above chart to the one from 6/19, it is clear that they are virtually identical.  I guess what this means is that, at least for the ‘free space’ case with no nearby interferers, calibration doesn’t do much.  Also, this implies that the heading errors observed above have nothing to do with external influences – they are ‘baked in’ to the magnetometer itself. The good news is, a sine function correction table should take most of this error out, assuming more accurate heading measurements are required (I don’t ).

In summary, at this point I have a working magnetometer calibration tool, and I have used it successfully to generate calibration matrix/center offset values for my Mongoose IMU’s HMC5883 magnetometer component.  After calibration, the ‘free space’ heading performance is essentially unchanged, as there were no significant ‘hard’ or ‘soft’ iron interferers to calibrate out.

Next up – remount the Mongoose on my 4WD robot, where there are plenty of hard/soft iron interference sources, and see whether or not calibration is useful.



Magnetometer Calibration, Part III

posted 06/14/16

In my last post on the subject of Magnetometer Calibration, I described an entirely complete and wonderful calibration utility I wrote in C#/.NET using Windows Forms, an old version of devDept’s EyeShot 3D viewport library, and calls into the Octave libraries to execute a MATLAB calibration script.  Unfortunately, at the end of the project I discovered to my horror that my redistribution rights for the EyeShot libraries had expired some time ago, and re-upping them was prohibitively expensive – so I could use my masterpiece, but no one else could! :-(.

So, it was ‘back to the drawing board‘ for me.  I needed a (ideally free) 3D visualization capability with reasonably easy-to-implement view manipulation (pan, zoom, rotate, coordinate axis, etc) tools that was compatible with C#/.NET.    After a fair bit of research, I found that Microsoft’s WPF (Windows Presentation Framework) platform advertised ‘full’ 3D visualization capability, so that was encouraging. Unfortunately, I had never used WPF at all, doing all my C#/.NET programming in the Windows.Forms namespace.  There were some posts that suggested the ability to place a WPF 3D viewport window into a Forms-based app via a ‘WindowsFormsHostingWpfControl’ but after trying this a bit I decided it was going to be too hard to build up the required 3D viewport and associated view manipulation tools ‘from scratch’.  Eventually I ran across the 3D Helix Toolkit at, and this looked very promising, but with the downside of having to re-create the entire application in WPF-land.  Actually, this appealed to me in a masochistic sort of way, as I would have the opportunity to learn two completely new packages/skills – WPF programming in general (which I had been ignoring for years in the hopes it would go away) and the feature-rich (but somewhat rough around the edges) Helix Toolkit.

So, off I went, reading as much as I could get my hands on about WPF and .NET visual programming.  It was initially very difficult to wrap my head around the way that WPF combines XAML with C# ‘code-behind’ to achieve the desired results.  At first I started out trying desperately to stick to my WinForms technique of drag/dropping tools onto a work surface, and then modifying properties as desired.  This worked up to a point, but I rapidly got lost due to the marked difference between WinForm’s ‘everything is a child of the main window’ and WPF’s hierarchical layout as described in XAML philosophy.  So, my first effort to build a WPF app isn’t very pretty, and definitely violates any number of rules for WPF elegance!  However, the use of WPF and the Helix Toolkit made it reasonably easy to implement the ‘raw’ and ‘calibrated 3D views, and I had no real trouble porting the comm port and Octave implementation logic from my previous app to this one.  And of course, the entire object of the exercise was to create an app that could be shared, and the WPF version (hopefully) does that.

My plan for the future – at least with respect to the Magnetometer Calibration Utility, is to share the app within the robotics/drone community, and to continue to support it as necessary to fix bugs and/or implement requested enhancements.  I also plan to set up a public GitHub repository as soon as I can figure out how to do it ;-).



Magnetometer Calibration, Part II

Posted 06/13/16

In my last post on this subject back in April, I had managed to figure out that my feeble attempts to compensate my on-robot magnetometer for hard/soft iron errors wasn’t going to work, and I was going to have to actually do a ‘whole sphere’ calibration to get any meaningful azimuth values from my magnetometer as installed on my robot.

As noted back in April, I had discovered two different tools for full-sphere magnetometer calibration (Yuri Matselenak’s program from the DIY Drones site, and Alain Barraud’s MATLAB script from 2008), but neither of them really filled the bill for an easy-to-use GUI for dummies like me.  At the end of April’s post, I had actually built up a partial GUI based on devDept’s EyeShot 3D viewport technology that I had lying around from a previous lifetime as a scientific software developer.  All I had to do to complete the project was to figure out how to integrate Alain’s MATLAB code into the EyeShot-based GUI and I’d be all set – or so I thought! ;-).

Between that last post in April and now, I have been busy with various insanities – competitive bridge, trying to develop a 3-point basketball shot, and generally screwing off, but I did manage to spend some time researching the issue of MATLAB-to-C# code porting.  At first I thought I would be able to simply port the MATLAB code to C# line-by-line.  I had done this in the past with some computational electromagnetics codes, so how hard could it be, anyway?  Well, I found out that it was pretty fricking hard, as in effectively impossible – at least for me; I just couldn’t figure out how to relate the advanced matrix manipulations in Alain’s code to the available math tools in C#.  I even downloaded the Math.NET Numerics toolkit from and tried it for a while, but I just could not make the connection between MATLAB matrix manipulation concepts and the corresponding ones in the Numerics toolkit – argghhh!!!.

After failing miserably, I decided to try and skin the MATLAB cat a different way.  I researched the Gnu Octave community, and discovered that not only was there a nice Octave GUI available for windows, but that some developers had been successful at making calls into the Octave dll’s from C# .NET code – exactly what I needed!

So, it was full steam ahead (well, that’s not saying much for me, but…) with the idea of a C#.NET GUI that used my EyeShot 3D viewport for visualization, and Octave calls for the compensation math, and within a few weeks I had the whole thing up and running – a real thing of beauty that I wouldn’t mind sharing with the world, as shown in the following video clip.


Unfortunately, after doing all this work I discovered that my EyeShot redistribution license for the 3D viewport library had long since expired, and although I can run the program happily on my laptop, I can’t distribute the libraries anywhere :-(((((.

Ah, well, back to the drawing board!


(author’s note: Although I did this work back in the April/May timeframe, I didn’t post about it until now.  I decided to go ahead and post it now as a ‘prequel’ to the next post about my ‘final solution’ to the magnetometer calibration utility challenge)