Giving Wall-E2 A Sense of Direction, Part VI

Posted July 06, 2016

In my last post  on this subject, I had used my newly-completed Magnetometer Calibration Tool to generate calibration factors for my HMC5883L-based ‘Mongoose IMU board, and compare the ‘raw’ vs ‘calibrated’ performance in a ‘free-space’ (actually my wood lab workbench) environment.  The result of the comparison showed  that the  ‘calibrated’ performance was pretty much unchanged from the ‘raw’ setup, indicating that the test setup (on my wooden workbench) wasn’t significantly affected by ‘hard’ or ‘soft’ interference.

The next step is to mount the Mongoose IMU on Wall-E2, my 4WD wall-tracking robot to see if the magnetometer can be compensated for DC motor magnet fields, power cables, and the like.  I decided to start this process by mounting the IMU on a wooden ‘stalk’ on the second deck, to see if this placement would minimize the above interfering effects.

Mongoose IMU mounted on wooden stalk on 2nd deck

Mongoose IMU mounted on wooden stalk on 2nd deck

Raw and calibrated data. Reference circles on left have radii equal to average raw value radius. Circles on right all have a radius == 1

Raw and calibrated data. Reference circles on left have radii equal to average raw value radius. Circles on right all have a radius == 1

The calibration values can now be saved to a text file convenient for transcription into the user’s calibration routine.  After doing the save, the text file looks like the following;

After copy/pasting the above values into my calibration routine and re-running the data collection exercise but recording the calibrated magnetometer readings instead, I got the following ‘raw’ (calibrated magnetometer data, but displayed in the ‘raw’ view) results.

The displayed data in the 'raw' view is new magnetometer data after being calibrated with the results of the first run. The circle radius on the left is 0.92. The data on the left is the old magnetometer data, calibtrated using the results of the calibration value computation from the first set of raw magnetometer data

Comparison of new calibrated data from the magnetometer with the results of the Octave calibration algorithm as applied to the old set of raw magnetometer data.

The displayed data in the ‘raw’ view is new magnetometer data after being calibrated with the results of the first run. The circle radius on the left is 0.92. The data on the left is the old magnetometer data, calibtrated using the results of the calibration value computation from the first set of raw magnetometer data.  As is easily seen from the two views, the calibration values generated by the Octave program produce very good ‘on-the-fly’ calibration results.

After calibration, I re-ran the heading performance tests (main power ON, but no drive to the motors), with the following results

Stalk-mounted magnetometer heading error, main power, no motor drive

Stalk-mounted magnetometer heading error, main power, no motor drive

The next step is to repeat this experiment with the motor drives enabled.  Here’s the results of a quick run.  With the motors enabled, I held Wall-E2 so that it’s wheels didn’t quite touch the surface, and slowly rotated the robot 360 clockwise, starting at the same point (nominally 0 deg as reported by the Mongoose IMU) as in the above plot.

Manually rotated over 360 degrees with motors running. Mongoose stalk mounted on 2nd deck

Manually rotated over 360 degrees with motors running. Mongoose stalk mounted on 2nd deck

As shown in the plot above, the headings reported by the Mongoose IMU increased monotonically as the robot was rotated clockwise from nominal zero. Although just a preliminary result, it  is actually quite encouraging, as it indicates that running the motors doesn’t significantly affect the heading value reported by the Mongoose IMU.

Today I had the chance to perform a ‘motors running’ heading error experiment with the stalk-mounted Mongoose IMU.  The robot body was placed on a small plastic box such that the wheels were free to turn without touching the workbench.  Then it was manually rotated in 10 deg increments as before.  The experimental setup and the results are shown below.

Test setup for the "Power and Motors" IMU heading error experiment.

Test setup for the “Power and Motors” IMU heading error experiment.

Stalk-mounted Mongoose IMU, with power and motor drive enabled.

Stalk-mounted Mongoose IMU, with power and motor drive enabled.

Comparing the heading error plots, it is pretty clear that enabling the motors does not significantly affect the stalk-mounted IMU.  If I wanted to leave the IMU mounted on the stalk, it appears that I could expect to get reasonable, if not spectacularly accurate, magnetic heading readings ‘in real life’.

However, I really  don’t want to leave the IMU mounted on a stalk, so the next step in the process will be to replace the stalk mounting arrangement with a more ‘streamlined’ mounting setup.  For this I plan to use the mounting bracket I printed up for the original front-mounted setup (see image below), but attached to the 2nd deck vs the 1st.

160318MongooseInstalled1_Annotated2

Original mounting location for the Moongoose IMU (arrow points to the IMU)

 

 

 

 

 

 

 

Magnetometer Calibration Tool, Part IV

In my  last episode of the Magnetometer Calibration Tool soap opera, I had a ‘working’ WPF application that could be used to generate a 3×3 calibration matrix and 3D center offset value for any magnetometer capable of producing  3D magnetometer values  via a serial port.  Although the tool worked, it had a couple of ‘minor’ deficiencies:

  • My original Eyeshot-based tool sported a very nice set of 3D reference circles in both the ‘raw’ and ‘calibrated viewports.  In the ‘raw’ view, the circle radii were equal to the average 3D distance of all  point cloud points from the center, and in the ‘calibrated’ view the circle radii were exactly 1.  This allowed the user to readily visualize any deviations from ideal in the ‘raw’ view, and the (hopefully positive) effect of the calibration algorithm.  This feature was missing from the WPF-based tool, mainly because I couldn’t figure out how to do it :-(.
  • The XAML and ‘code-behind’ associated with the project was a god-awful mess!  I had tried lots and lots of different things while blindly stumbling toward a ‘mostly working’ solution, and there was a  LOT of dead code and inappropriate structure still hanging around.  In addition to being ugly, this state of affairs also reflected my (lack of) understanding of basic WPF/Helix Toolkit concepts, principles, and methods.

So, this post describes my attempts to rectify both of these problems.  Happily, I can report that the first one (lack of switchable reference circles) has been completely solved, and the second one (god-awful mess and lack of understanding) has been at least partially rectified; I have a much better (although not complete by any means!) grasp of how XAML and ‘code-behind’ works together to produce the required visual effects.

To achieve better understanding of the connection between the 3D viewport implemented in Helix Toolkit by the HelixViewport3D object, the XAML that describes the window’s layout, and the ‘code-behind’ C# code, I spent a lot of quality time working with and modifying the Helix Toolkit’s ‘Simple Demo’ app.  The ‘Simple Demo’ program displays 3 box-like objects (with some spheres I added) on a grid, as shown below

Simple Demo WPF/Helix Toolkit Application

Simple Demo WPF/Helix Toolkit Application (spheres added by me)

Simple Demo XAML View

Simple Demo XAML View – no changes from original

Simple Demo 'Code-behind', with my addition highlighted

Simple Demo ‘Code-behind’, with my addition highlighted

My aim in going back to the ‘Simple Demo’ was to avoid  the distraction of my more complex window layout (2 separate HelixViewport3D windows and  lots of other controls) and the associated C#/.NET code so I could concentrate on one simple task – how to  implement a set of 3D reference circles that can be switched on/off via a windows control (a checkbox in my case).  After trying a lot of different things, and with some clues garnered from the Helix Toolkit forum, I settled on the TubeVisual3D object to construct the circles, as shown in the following screenshots.  I used an empirically determined ‘thickness factor’ of 0.05*Radius for the ‘Diameter’ property to get the ‘thick circular line’ effect I wanted.

Simple Demo modified to implement TubeVisual3D objects

Simple Demo modified to implement TubeVisual3D objects.  The original box/sphere stuff is still there, just too small to see

MyWPFSimpleDemo 'code-behind', with TubeVisual3D implementation code highlighted

MyWPFSimpleDemo ‘code-behind’, with TubeVisual3D implementation code highlighted.  Note all the ‘dead’ code where I tried to use the EllipsoidVisual3D model for this task.

Next, I had to figure out a way of switching the reference circle display on and off using a windows control of some sort, and this turned out to be frustratingly difficult.  It was easy to get the circles to show up on program startup – i.e. with model construction and the connection to the viewport established in the constructor(s), but I could not figure out a way of doing the same thing after the program was already running.  I knew this had to be easy – but damned if I could figure it out!  Moreover, after hours of searching the blogosphere, I couldn’t find anything more than a few hints about how to do it. What I  did find was a lot of WPF beginners like me with the same problem but no solutions – RATS!!

Finally I twigged to the fundamental concept of WPF 3D visualization – the connection between a WPF viewport (the 2D representation of the desired  3D model) and the ‘code-behind’ code that actually represents the 3D entities to be displayed must be defined at program startup, via the following constructs:

  • In the XAML, a line like  ‘<ModelVisual3D Content=”{Binding Model}”/>, where Model is the name of a  Model3D property declared in the  ‘code-behind’ file (MainViewModel.cs in my case)
  • In MainWindow.xaml.cs, a  line like ‘this.DataContext = mainviewmodel’, where mainviewmodel is declared with ‘public MainViewModel mainviewmodel = new MainViewModel();’
  • In MainViewModel.cs, a line like ‘ public Model3D Model { get; set; }’, and in the class constructor, ‘Model = new Model3DGroup();’
  •  in MainViewModel.cs, the line ‘var modelGroup = new Model3DGroup();’ at the top of the model creation section to create a temporary Model3DGroup object, and the line ‘ this.Model = modelGroup;’ at the bottom of the model construction code. This line sets the Model property contents to the contents of the temporary modelGroup‘ object

So, the ‘MainViewModel’ class is connected to the Windows window  class in MainWindow.xaml.cs, and the 3D model described in the MainViewModel class is connected to the 3D viewport via the Model Model3DGroup object.  This is all done at initial object construction, in the various class constructors.  There are still some parts of this that I do not understand, but I think I have it mostly correct.

The important concept that I was missing  is the above connections have been made at program startup and cannot (AFAICT) be changed once the program starts, but the contents of the temporary  Model3DGroup object (i.e. the ‘Children’ objects in the model group) can be changed, and the new contents will be reflected in the viewport when it is next updated.  Once I understood this concept, the rest, as they say, “was history”.  I implemented a simple control handler that cleared the contents of the temporary Model3DGroup object modelGroup and regenerated it (or not, depending on the state of the ‘Show Ref Circles’ checkbox).  Simple and straightforward, once I knew the secret!

So this ‘aha’ moment allowed me to implement the switchable reference circles in my Magnetometer calibration tool and check off the first of the deficiencies noted at the start of this post.  The new reference circle magic is shown in the following screenshots.

Raw and calibrated magnetometer data. Calculated average radius of the raw data is about 444 units, and the assumed average radius of the calibrated data is close to 1 unit

Raw and calibrated magnetometer data. Calculated average radius of the raw data is about 444 units, and the assumed average radius of the calibrated data is close to 1 unit

Raw and calibrated magnetometer data, with reference circles shown. The radius of the 'raw' circles is equal to the calculated average radius of about 444 units, and the assumed average radius of the calibrated circles is exactly 1 unit

Raw and calibrated magnetometer data, with reference circles shown. The radius of the ‘raw’ circles is equal to the calculated average radius of about 444 units, and the assumed average radius of the calibrated circles is exactly 1 unit

The reference circles make it easy to see how the calibration process affects the data.  In the ‘raw’ view, it is apparent that the data is significantly offset from center, but still reasonably spherical.  In the calibrated view, it is easy to see that the calibration process centers the data, removes most of the non-sphericity, and scales everything to very nearly 1 unit – nice!

Now for addressing the second of the two major deficiencies noted at the start of this post, namely “The XAML and ‘code-behind’ associated with the project was a god-awful mess! “.

With my current understanding of a typical WPF-based application, I believe the application architecture consist of three parts – the XAML code (in MainWindow.xaml)    that describes the window layout,  the ‘MainWindow’ class (in MainWindow.cs) that contains the interaction logic with the main window, and a class or classes that generate the 3D models to be rendered in the main window.  For  my magnetometer calibration tool  I created  two 3D model generation classes – ViewportGeometryModel and RawViewModel.  The ViewportGeometry class is the base class for RawViewModel, and handles generation of the three orthogonal TubeVisual3D ‘circles.  The  ViewportGeometryModel class is instantiated directly (as ‘calmodel’ in the code) and connected to the main window’s ‘vp_cal’ HelixViewport3D window via it’s ‘GeometryModel’ Model3D property, and the derived class RawViewModel (instantiated in the code as ‘rawmodel’) is similarly connected to the main window’s ‘vp_raw’ HelixViewport3D window via the same  ‘GeometryModel’ Model3D property (different object instantiation, same property name).

The ViewportGeometryModel class has one main function, and some helper stuff.  The main function  is  ‘DrawRefCircles(HelixViewport3D viewport, double radius = 1, bool bEnable = false)’.  This function is called from MainWindow.xaml.cs as follows:

The ‘DrawRefCircles()’ function creates a new ModelGroup3D object if necessary, and optionally fills it with three TubeVisual3D objects of the desired radius and thickness, as shown below

The last line in the above function is ‘GeometryModel = modelGroup;’, where ‘GeometryModel’ is declared in the ViewGeometryModel class as

and bound to the appropriate HelixViewport3D window via

Line in MainWindow.xaml that binds the HelixViewport3D to the 'GeometryModel' Model 3D property of the ViewportGeometryModel class

Line in MainWindow.xaml that binds the HelixViewport3D to the ‘GeometryModel’ Model 3D property of the ViewportGeometryModel class (and/or its derived class RawViewModel). The line shown here is for the raw viewport, and there is an identical one in the calibrated viewport section.

Now, instead of a mishmash spaghetti factory, the program is a lot more organized, modular, and cohesive (or at least I think so!).  As the following screenshot shows, there are only a few classes, and each class does a single thing.  Mission accomplished!

Magnetometer calibration tool class diagram. Note that the RawViewModel is a derived class from VieportGeometryModel

Magnetometer calibration tool class diagram. Note that the RawViewModel is a derived class from VieportGeometryModel.  The ViewportGeometryModel.CirclePlane ‘class is an Enum

Other Stuff:

This entire post has been a description of how I figured out the connections between a WPF-based windowed application with two HelixViewport3D 3D viewports (and lots of other controls) and the XAML/code-behind elements that generate the 3D models to be rendered. In particular it has been a description of the ‘reference circle’ feature for both the ‘raw’ and ‘calibrated’ views.  However, these circles are really only a small part of the overall magnetometer calibration tool; a much bigger part of the 3D view are  the point-clouds in both the raw and calibrated views that depict the actual 3D magnetometer values acquired from the magnetometer being calibrated, before and after calibration.  I didn’t say anything about these point-cloud collections, because I had them working long before I started the ‘how can I display these damned reference circles’ odyssey.  However, I thought it might be useful to point out (no pun intended) some interesting tidbits about the point-cloud implementation.

  • I implemented the point-cloud using the Helix Toolkit’s PointsVisual3D and Point3DCollection objects.  Note that the PointsVisual3D object is derived from ScreenSpaceVisual3D which is derived from RenderingModelVisual3D  instead of a geometry object like TubeVisual3D which is derived from  ExtrudedVisual3D, which in turn is derived from  MeshElement3D.   These are very different inheritance chains.  A  PointsVisual3D object can be added directly to a HelixViewport3D object’s Children collection,  and doesn’t need a light for rendering!  I can’t tell you how much agony this caused me, as I just couldn’t understand why other objects added via the ModelGroup chain either didn’t render at all, or rendered as flat black objects.  Fortunately for me, the ‘SimpleDemo’ app  did have light already defined, so things displayed normally (it still took me a while to figure out that I had to add a light to my MagCal app, even though the point-cloud displayed fine).
  • Points in a point-cloud collection don’t support a ‘selected’ property, so I had to roll my own selection facility.  I did this by handling the mouse-down event, and manually checking the distance of each point in the collection from the mouse-down point.  If I found a point(s) close enough, I manually moved the point from the ‘normal’ point-cloud to a ‘selected’ point-cloud, which I rendered slightly larger and with a different color.  If a  point became ‘unselected’, I manually moved it back into the ‘normal’ point-cloud object.  A bit clunky, but it worked.

All of the source code, and a ZIP file containing everything (except Octave) needed to run the Magnetometer Calibration app is available at my GitHub site –  https://github.com/paynterf/MagCalTool

Frank

 

 

 

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.

160318MongooseInstalled1_Annotated2

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:

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

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  http://www.helix-toolkit.org/, 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  http://numerics.mathdotnet.com/ 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!

Frank

(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)

 

 

Magnetometer Calibration, Part I

Posted 20 April, 2016

I have been trying to add a magnetometer to my Wall-E2 robot for a while now, and have been plagued by installation-induced errors.  A magnetometer works fine on the bench in isolation, but not when installed on the robot.  So far, I have determined that the DC drive motors are a big contributor to these errors, and the only way to address this is with magnetometer calibration.  At first I tried just a simple lookup-table based approach, since I only needed to correct for azimuth errors (my wheeled robot very rarely departs from a horizontal orientation).  Again, this worked great ‘on the bench’, but failed miserably in the installed configuration.

So, after being forcibly convinced that ‘the easy way’ wasn’t going to work, I began researching magnetometer calibration techniques and tools.  At the DIY Drones website, I found a very informative article by Yury Matselenak  with a great explanation, and a set of two tools (a visualizer and a calibration tool).  I thought I was ‘in like Flynn’ and downloaded the tools.  Unfortunately the visualizer tool didn’t recognize comm two-digit port port numbers, so once again I was stuck (Yuri later gave me a link to the visualizer source code, but I haven’t yet had the time to fix it for the higher numbered ports).  In the meantime, I found another calibration tool, written in MATLAB by Alain Barraud, so I decided to try my hand at a magnetometer calibration manager.  I have a fair bit of experience with MATLAB from a prior lifetime as a researcher, and I have previously ported MATLAB code to C++, so how hard could it be?  From a previous project I had access to an older version the neat EyeShot 3D visualization libraries, so it was easy to add a 3D viewport to a standard C# Windows form.  My plan was to have a ‘raw’ and a ‘calibrated’ view, so it would be easy to see the effects of the calibration process, and to use the free MathNet.Numerics matrix manipulation tools to port the MATLAB code.  Unfortunately, the  MATLAB matrix manipulation routines used in the calibration code didn’t correspond closely enough to  MathNet’s library functions, so I got stuck during the port and wasn’t able to finish the ‘calibrated’ side of the application – bummer!

However, I did get far enough along to notice another problem; the raw data from my magnetometer exhibited a lot of ‘spread’ in one of the principal planes, where a small percentage of the data points formed a rough circle at about twice the radius as all the other points.  When I ran this dataset through Alain’s original MATLAB code (using Octave on my Linux box) the ‘calibrated’ dataset actually looked worse than the original.  This prompted me to add some point editing capability to my visualizer, so I could visually remove ‘outrider’ data prior to attempting calibration.

Raw vs Calibrated data before pruning outliers

Raw vs Calibrated data before pruning outliers

Raw vs Calibrated data after pruning outliers

Raw vs Calibrated data after pruning outliers

Here are some shots of the process of pruning the dataset using my calibration app

Raw magnetometer data before any pruning

Raw magnetometer data before any pruning

All data beyond a settable radius selected

All data beyond a settable radius selected

All selected outliers removed

All selected outliers removed

Pruned data written to text file for later processing through MATLAB calibration app

Pruned data written to text file for later processing through MATLAB calibration app

So, even though I couldn’t (and still can’t) figure out how to port Alain’s MATLAB code into my calibration tool, I  was able to use the tool to visually prune outliers from my data prior to running it through the calibration routine, and so all my work wasn’t a complete waste – whoopee! (not).

Frank

 

 

Giving Wall-E2 A Sense of Direction, Part IV

Posted 03/20/16

In my last post, I described my efforts to integrate a CK Devices Mongoose 9DOF IMU module into my Wall-E2 wall-following robot. In that post, I had collected  a set of azimuth (heading) data from the Mongoose showing that the Mongoose was operating properly and that significant error compensation was possible using a simple sine function.  This led me to believe that it would be feasible to install the Mongoose on the robot and create a similar compensation function.  Unfortunately, that turned out not to be the case.  When I collected a similar set of heading data in the installed case, the data showed non-linear behavior for which function-based compensation  would be difficult, if not impossible to achieve.

The image  below shows results from the ‘bare’ (desktop) uninstalled configuration, where it is clear that the Mongoose raw heading values closely follow the actual magnetic heading, and that a simple sine function is sufficient to reduce heading error to approximately +/- 2 deg.

Mongoose Mag Heading & Error on desk before installation on robot

Mongoose Mag Heading & Error on desk before installation on robot

The next set of  images shows the Mongoose installed on the robot between the upper and lower decks.  The idea was to place the Mongoose in a location reasonably well protected physically, but away from high-current elements.  I also wanted to avoid placing it on the upper deck to avoid the additional inter-deck wiring and attendant maintenance/troubleshooting complexity.

Mongoose installed on robot, side view

Mongoose installed on robot, side view

Mongoose installed on robot, top view

Mongoose installed on robot, top view

160318MongooseInstalled1_Annotated2

 

After installation, the same set of measurements were taken, with the results shown in the following plots.  As can be seen, the Mongoose heading readings in this case are hugely different than the desktop run.  Instead of being able to compensate the heading error to within +/- 2 deg, the compensated error is greater than the uncompensated one!  Looking at the Installed Raw Heading plot, it appears that the readings are relatively linear (but heavily suppressed) out to about 315 deg, where something bad happens and the reported heading falls rapidly to near zero.  I made another run in the installed configuration with the magnetometer gain turned down as far as possible, but this did not materially improve the situation.  Clearly something on the robot was drastically affecting the Mongoose magnetometer, to the extent that compensation was  impossible. Moreover, due to the retrograde readings between 315 and 360 degrees, even a lookup table solution seems problematic.

160318MongooseInstalledHdgCal1Results

As I often do when faced with what appears to be an insurmountable obstacle, I tabled the problem and did something else for a while and let my subconscious work on the problem for a while.  After a couple of days of this, I decided to go back to the uninstalled (desktop) case, re-establish my measurement baseline, and then see if I could determine what on the robot was causing such huge magnetic heading variations.   After playing around for a while, I was able to determine that the cause of the problem was the permanent magnets in the DC wheel motors – well DUH!!  After smacking myself on the forehead a couple of times for not thinking of this days ago, I realized that I had carefully determined that Wall-E’s chassis was constructed of aluminum that shouldn’t (I thought) cause problems with the magnetometer, forgetting entirely the fact that in addition to not affecting the magnetometer, it also wouldn’t shield the magnetometer from the strong magnetic fields generated by the motor magnets – oops!

So, what to do?  As much as I would like to avoid it, it appears now that the only viable solution (other than abandoning the magnetometer idea entirely) is to put the Mongoose on the top deck, as far away from the motors as possible.  I am at least a little optimistic that this will work, for two reasons; with the gain turned down, the Mongoose  almost worked where it was, and because mag fields decrease with R3, a few cm could make a significant difference.

Stay tuned!

Frank

 

 

 

Giving Wall-E2 A Sense of Direction. Part III

Posted  March 14, 2016

In my last post from about a week ago, I described my ongoing efforts to integrate the CK Devices Mongoose 9DOF IMU into my ‘Wall-E2’ wall-following robot.  Since that time, I have gotten the Mongoose successfully integrated into the robot, and am able to see magnetometer & accelerometer readings being passed through the host Arduino Mega to the PC via the Mega’s serial port.

After getting all the hardware and software issues worked out, I have now started on the issue of getting the magnetometer calibrated for it’s new home in the robot.  All magnetometers need to be calibrated after installation to compensate for errors caused by nearby metal (ferrous and non-ferrous) objects; otherwise the reported magnetic heading can be substantially off.  I have considered just using a 360-element lookup table containing offset values, but that’s a bit tacky even for one with my low standards, so I have been researching available magnetometer calibration techniques and tools.  I found a nice  discussion at DIY Drones here, but I have been having trouble getting the tools to work.  The discussion (and tools) center around the widely available  GY-273 HMC5883L  breakout board, and this ain’t quite the same animal as the Mongoose.

Following the general line of the discussion at DIY Drones, I downloaded the ‘MagMaster’ ZIP files, and attempted to get the MagViewer visualizer program linked up with my Mega/Mongoose combination, without much success.  After flailing around for a while with the robot/Mongoose setup, I decided to simplify things by isolating the Mega/Mongoose combination from everything else going on with the robot.  I had a spare Mega, so I simply connected the Mongoose to Tx1/Rx1 on the spare (same as on the robot) and loaded a modified version of the robot controller onto the Mega.

MongooseCalSetup1

The modifications basically stripped away everything to do with the robot, leaving only the code that interfaces with the Mongoose.  According to the DIY Drones discussion, this  should have allowed the MagViewer program to see the same magnetometer data as for the GY-273, but apparently not.  The viewer program never shows any activity – bummer!

Posted  March 16, 2016

Well, I’m still not sure why the MagViewer program didn’t recognize the Mongoose mag data (I posted to the DIY Drones forum, but no replies yet), so  I decided I would try a variant on my original idea of a lookup table.

First, I needed a way to accurately orient  my Mongoose module to different headings.  I Googled around a bit, and found a protractor printing site  that offered 360-degree heading graphics like the one shown below

4-inch heading circle, calibrated in degrees

4-inch heading circle, calibrated in degrees

Then I taped a narrow strip of paper to the bottom centerline of the Mongoose module to make an accurate pointer, and proceeded to record the actual Mongoose heading reading for each 5-degree increment around the circle.  I started this process by orienting the paper heading circle so that the 0/360 point corresponded to a Mongoose heading reading of 0 degrees, i.e. aligned with magnetic north as measured by the Mongoose.  The data were recorded in an Excel spreadsheet, and the error term (difference between the nominal heading value and the Mongoose reading) was calculated and graphed, as shown below.

 

Mongoose reading versus actual magnetic heading, with error plot

Mongoose reading versus actual magnetic heading, with error plot

Well, when the graph first popped up, I just about fell off my chair, as I recognized an almost perfect sine wave graph.  This immediately told me two things:

  1. The Mongoose sensor, the test setup and the data was all valid.  There is no way I could have managed that smooth of a curve by accident, and also no way it could have been that smooth if there was any significant mag field distortion
  2. At least for this case, with no significant installation errors, almost perfect heading compensation could be accomplished with just a simple sine function with a slight negative DC offset corresponding to the average value of all errors (about -1.35 according to Excel)

To test this theory, I  used Excel to calculate the required sin function values, and added the result to the calculated error values for each measured angle.  Then I plotted the compensation and comp+error curves on the same plot as before, as shown below.

Angle Error, Comp values, and Comp+Error

Angle Error, Comp values, and Comp+Error

From the plot, it is clear that the compensation  is effective, although not perfect. The compensated error amplitude looks to be about 2-2.5 degrees, more than adequate for my purposes.  I think the remaining error is due to the fact that the sensor data traces out an ellipse rather than a circle.

So, the next step is to install the Mongoose sensor back on the robot, and do a similar test utilizing an 8″ diameter version of the heading graphic.  Assuming I get similar compensation results, I’ll probably call it a day and start running field tests.  After all, I’m not sure I care if Wall-E thinks a hallway is oriented at 270, 260, or 280 degrees magnetic, as long as the next time it goes down the same hallway it gets more or less the same results!

Stay tuned!

Frank

 

 

 

 

 

Giving Wall-E2 A Sense of Direction – Part II

Posted 03/07/16

In my last post on this subject, I described my efforts to renew my acquaintance with CK Devices’ Mongoose 9DOF IMU board, with the intent of integrating it into my wall-following robot project.  This post describes the first step toward that integration goal.

The Mongoose board operates on 3.3V and communicates via RS-232 protocol over a 6-pin 0.1″ header block, compatible with the CK Devices ‘FTDI Pro’ programmer  module, as shown in the following photo.

Mongoose 9DOF IMU board, with FTDI Pro USB-Serial adapter

Mongoose 9DOF IMU board, with FTDI Pro USB-Serial adapter

Integrating this board into Wall-E requires addressing two distinct but related issues – the voltage difference between the 5V Arduino Mega controller and the 3.3V Mongoose, and  the coding changes required to retrieve the desired heading/attitude information from the Mongoose.

The Hardware

The 5V/3.3V issue requires that 5V serial data levels from the Arduino Mega be stepped down to 3.3V, and 3.3V serial data levels from the Mongoose be stepped up to 5V.  Simply ignoring these problems does seem to work (the 5V data from the Mega doesn’t appear to harm the Mongoose, and the 3.3V data from the Mongoose does seem to be recognized by the Mega), but it is inelegant to say the least.  Also, a recent addition to Wall-E’s superpowers included Pololu’s  ‘Wixel Shield’ module, which fortuitously included some spare circuitry for just this purpose, as shown in the schematic drawing excerpt below.  The upper right corner shows the full step-up circuit used to connect the 3.3V Wixel Tx line to the 5V Arduino Rx line, and the top  center  shows the step-down circuit used to connect the 5V Arduiono Tx line to the 3.3V Wixel Rx line.  The lower area shows the spare parts available on the Wixel shield – and the important thing is that there are  two general-purpose N-channel MOSFETs. These can be used to construct the same step-up circuit to connect the 3.3V Mongoose Tx line to the 5V Arduino Mega Rx1 line, and one of the 4 available 2/3 voltage dividers can be used to step down the 5V Arduino Mega Tx line to the 3.3V Mongoose Rx line.

SchematicDetail

The following image  is a small section of my overall Scheme-it (Digikey’s free online schematic capture app) schematic for Wall-E, showing the addition of the Mongoose IMU.

MongooseTxRxDetail

The photo below shows the serial up/down converter connections on the Wixel Shield board.  On the near side, Red = Mongoose Rx, Org = Mongoose Tx.  On the far side, Org = Mega Rx1, White = Mega Tx1.

Mongoose Serial Up/Down Converter Connections.  Near side Red = Mongoose Tx, Org = Mongoose Rx.  Far side: Org = Mega Rx1, White = Mega Tx1

Mongoose Serial Up/Down Converter Connections. Near side Red = Mongoose Tx, Org = Mongoose Rx. Far side: Org = Mega Rx1, White = Mega Tx1.  Yel = Mongoose 3.3V, Grn = Mongoose Gnd.

 

The Software:

After getting the hardware wired up and tested, I naturally thought All I had to do was plug in the Mongoose with the full set of reporting software, and I’d be ready to go –  NOT!  So, after the obligatory yelling and cursing and the appropriate number of random code changes hoping something would work, I was forced to fall back on my somewhat rusty (but not completely worthless) trouble-shooting skills.  I started by creating a new, blank Arduino project.  Into this project I placed code to simply echo whatever I typed on the serial command line to the serial view window.  To this I added a few lines to make the laser pointer blink on/off, just to provide a visual “I’m alive!” indication, and then tested it.  After all errors were made/corrected, etc, the final code was as shown below.

asd;lfkjsaf;
al;skfdj

The next step was to extend the echo loop to include Serial1, with a simple jumper between Tx1 and Rx1 on the Mega board.  In the software all this required was Serial1.begin(9600) in Setup() and the addition of the code to route command line bytes out Tx1 and to route received bytes on Rx1 to Tx0 so that it would get displayed on the serial monitor.  The code to do this is shown below:

The next step was to extend the loop beyond Serial1 and into the Mongoose, by replacing the jumper from Tx1 to Rx1 with the Mongoose board’s Rx & Tx lines respectively, and replacing the normal Mongoose code with the turn-around (echo) code.  No change should be required to the Arduino code, as it will still be echoing whatever shows up at Rx1 to Tx0.

The Mongoose code is:

 

When this code was loaded into the Mongoose  and run stand-alone, I got the following output by typing ‘this is a test’ on the serial port command line.

MongooseEcho1

After  the Mongoose was disconnected and placed into the robot circuit, I got the following output from the Robot project on a different comm port.

MongooseEcho2

So, it is clear from the above that the Mongoose board connected (through the step-up/down circuitry on the Wixel shield) to Rx1/Tx1 is successfully receiving and echoing whatever shows up on its serial port.

Now it was time to reload my modified version of CK Devices ‘Mongoose_base’ firmware back onto the Mongoose and get it running with Wall-E’s firmware.  After the usual number of hiccups and such, I got it running and nicely reporting ‘Tilt’ (X-axis accelerometer) and ‘Heading’ (derived from 3-axis magnetometer values) along with the normal robot telemetry values, as shown in the printout below.

Final version showing Mongoose Tilt & Hdg values integrated into normal robot telemetry

Final version showing Mongoose Tilt & Hdg values integrated into normal robot telemetry

Although  the Mongoose module  has been successfully integrated into the robot system from a technical standpoint, there is still plenty of work to be done to complete the project.  At the moment (see the photo below), it is just kind of ‘hanging around’ (literally), so some additional work will be required to mount it properly.  Stay tuned! ;-).

Mongoose installed loosely on Wall-E2

Mongoose installed loosely on Wall-E2