I have been struggling with how to fuse all the data into one cohesive mess. In the case of heading I have several source. I have a magnetometer, the heading from GPS, the integrated value from the gyro, and if I can manage it, the solar compass.
They each have strengths and weaknesses. The magnetometer is great as long as the car is not moving, the servos moving, or it sitting on a man hole cover.
The GPS is great as long as the robot is moving a a reasonable speed and has a good lock.
The gyro has no ability to determine north, but for short periods of time can keep track of changes in heading.
The solar compass is fantastic as long as we have the sun and it is not too high in the sky.
So what is a poor robot to do? One solution is to use a Kalman filter to merge the data. But the math is complex and you need mathematical model of your system to make best use of it. Now I have the habit of picking at everyone I know for knowledge (and even those I do not know). This has lead me to the complimentary filter.
The complimentary filter is nothing more than a weighted average. Rather than each heading having equal weight when taking an average, you assign each a unique weight. The more likely that the value is right the higher the weight you give it.
So how do you assign weights? The literature would have you use the parameter’s covariance. This is the inverse of the standard deviation. A parameter you have confidently measured would have a larger covariance than one you are not so sure about. There are lots of mathematical ways to determine covariance given sample sets of numbers. But in my case, I will take simpler way out.
My weights will be the inverse of the expected accuracy of the system. Here are the cases:
GPS: Zero if velocity is under 2 meters/second, else it ramps up to 3 degrees at 6 m/s or above.
Magentometer: 5 degrees if motor is off, 180 degrees if on
SUNDAR: Based on the error it reports based on the signal to noise ratio
IMU: Starts with the accuracy of the device that last reset its heading. The accuracy then decreases by 90 degrees per minute before it gets reset by the best azimuth device.
My heading now becomes the weighted average using the equation:
heading = (h1*W1 + h2*W2 + h3*W3 + h4*W4)/(W1+W2+W3+W4)
The problem is this does not work for headings. To see why assume two equally weighted sensors providing the headings of 350 and 10 degrees. The average would be (350*1 + 10*1)/(1+1) or 180 degrees! Oops; wrong direction! We really should have gotten 0 or 360 degrees.
So how does one average headings? A future post will address that.