Compass Instability Solution
Hi,
Is there a way to stabilize the compass readings?
I am working on a project that involves multiple robots and I need to derive their locations and also their orientations. The problem is that the compass readings are fluctuating a lot and also not all compasses on different robots give the same readings.
Is there a way to synchronize and stabilize the compass readings?
Thanks,
-Prashant
Is there a way to stabilize the compass readings?
I am working on a project that involves multiple robots and I need to derive their locations and also their orientations. The problem is that the compass readings are fluctuating a lot and also not all compasses on different robots give the same readings.
Is there a way to synchronize and stabilize the compass readings?
Thanks,
-Prashant
Comments
About the only way I can think to do something like that would be to have a compass standard, similar to a time standard. The only way I can think of to find such a compass standard might be to derive it from a GPS position. I suspect that might be cost prohibitive, since you're USING compasses rather than GPS to begin with.
Regards,
Bruce Bates
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Programming can't be all that difficult, it's nothing but 1's and 0's
Thanks for the reply.
Is there any previous work that you can direct me to, as I still do not have enough idea about what has to be done.
-Prashant
1) Use a known good compass or other geophysical standard (like GPS) to draw a large compass circle, then calibrate each of the robot compasses against that with a table of corrections for each direction or portions of the circle.
2) Average a bunch of readings on each robot. Do a statistical analysis of a series of readings to see how much they vary, over what timeframe, etc. That may tell you how many readings you'll need to average to get a direction that's useful.
3) Make sure to get your compass readings with the motors turned off. You'll still have the fixed magnetic fields of the motors distorting the readings, but at least they won't be fluctuating.
I am using HM 55B compass. The providers of this compass have given some code to get the x, y readings using the compass along an interference minimizing code.
Unfortunately; this code doesn't help calculate angles when the robot is moving. Have you worked or come across this situation?
The code is pasted below.
[noparse][[/noparse]code]
' ============================================================================
' TestCalibratedHM55BCompass.bs2 - Demonstrates Hitachi HM55B Compass Module's
' accuracy after calibration with CalibrateHM55BCompass.bs2.
'
' IMPORTANT: This program only works after calibration with CalibrateHM55B-
' Compass.bs2. For instructions on how to use these two programs
' look for the download entitled Calibrating the Compass Module on
' the Hitachi HM55B Compass Module product page at
' www.parallax.com.
'
' Author.... (C) 2005 Parallax, Inc -- All Rights Reserved
' Email..... support@parallax.com
'
' {$STAMP BS2}
' {$PBASIC 2.5}
' ============================================================================
'
[noparse][[/noparse] EEPROM Data ]
CompassOffsets DATA @ 0, (4) ' Stores x and y axis offsets
CompassLowVal DATA (1) ' Stores index of lowest angle
CompassCal DATA (16) ' 16 reference compass angles
'
[noparse][[/noparse] Pin Definitionis ]
DinDout PIN 7 ' P6 tranceives to/from Din/Dout
Clk PIN 6 ' P5 sends pulses to HM55B's Clk
En PIN 5 ' P4 controls HM55B's /EN(ABLE)
'
[noparse][[/noparse] Constants ]
Reset CON %0000 ' Reset command for HM55B
Measure CON %1000 ' Start measurement command
Report CON %1100 ' Get status/axis values command
Ready CON %1100 ' 11 -> Done, 00 -> no errors
NegMask CON %1111100000000000 ' For 11-bit negative to 16-bits
current CON 0 ' Table array index
previous CON 1 ' Table array index
'
[noparse][[/noparse] Variables ]
x VAR Word ' y-axis data
y VAR Word ' x-axis data
status VAR Nib ' Status flags
angle VAR Word ' Angle measurement
axisOffset VAR angle ' Axis offset
index VAR Status ' EEPROM index
table VAR Byte(2) ' Stores EEPROM table values
span VAR x ' Span between table entries
angleOffset VAR y ' Offset btwn measured and table
'
[noparse][[/noparse] Main Routine ]
DEBUG CLS
DO ' Main loop
GOSUB Get_Compass_Axes ' Get x, and y values
GOSUB Compass_Correct_Offsets ' Correct axis offsetes
angle = x ATN -y ' Convert x and y to brads
' DEBUG HOME, "x-axis N(-S) = ",SDEC x, ' Display corrected axes
' CLREOL, CR, "y-axis W(-E) = ",
' SDEC y, CLREOL
GOSUB Compass_Interpolate ' Linear interpolation
' DEBUG CR, CR, "angle = ", ' Display inrerpolated angle
' DEC angle, " brads", CLREOL ' ... in brads
angle = angle */ 360 ' Convert brads to degrees
DEBUG HOME,DEC3 angle ' Display inrerpolated angle
' DEC angle, " degrees", CLREOL ' ... in degrees
PAUSE 150 ' Debug delay for slower PCs
LOOP ' Repeat main loop
'
[noparse][[/noparse] Subroutine - Get_Compass_Axes ]
' This subroutine handles BASIC STamp - HM55B communicaton and stores the
' magnetic field strenth measurements returned by the device in the x and
' y axis variables.
Get_Compass_Axes: ' Compass subroutine
HIGH En: LOW En ' Disable/enable module
SHIFTOUT DinDout,clk,MSBFIRST,[noparse][[/noparse]Reset\4] ' Send reset command
HIGH En: LOW En ' Disable/enable module
SHIFTOUT DinDout,clk,MSBFIRST,[noparse][[/noparse]Measure\4] ' Start measurement
status = 0 ' Clear previous status flags
DO ' Status flag checking loop
HIGH En: LOW En ' Disable/enable sensor
SHIFTOUT DinDout,clk,MSBFIRST,[noparse][[/noparse]Report\4] ' Request status
SHIFTIN DinDout,clk,MSBPOST,[noparse][[/noparse]Status\4] ' Get Status
LOOP UNTIL status = Ready ' Exit loop when status is ready
SHIFTIN DinDout,clk,MSBPOST,[noparse][[/noparse]x\11,y\11] ' Get x & y axis values
HIGH En ' Disable module
IF (y.BIT10 = 1) THEN y = y | NegMask ' Store 11-bits as signed word
IF (x.BIT10 = 1) THEN x = x | NegMask ' Repeat for other axis
RETURN
'
[noparse][[/noparse] Subroutine - Compass_Correct_Offsets ]
' This subroutine corrects cumulative magnetic field interferance that can
' come from sources such as the PCB, jumper wires, a nearby batter, or a
' nearby current source. This subroutine relies on values stored in
' the EEPROM space that was reserved by the CompassOffsets DATA directive.
' These EEPROM values were writen by CalibrateHM55BCompass.bs2.
Compass_Correct_Offsets:
READ CompassOffsets, Word axisOffset ' Get x-axis offset
x = x - axisOffset ' Correct x-axis
READ CompassOffsets + 2, Word axisOffset ' Get y-axis offset
y = y - axisOffset ' Correct y-axis
RETURN
'
[noparse][[/noparse] Subroutine - Compass_Interpolate ]
' This subroutine applies linear interpolation to the refine the compass
' measurement. This second level of refinement can be performed after the
' Compass_Correct_Offsets subroutine, and it can correct axis skew and other
' errors inherent to the HM55B chip.
'
' The subroutine relies On sixteen actual compass measurements that were stored
' in the sixteen EEPROM locations reserved by the CompassCal DATA directive.
' These measurements were stored by CalibrateHM55BCompass.bs2, and they
' represent the actual compass measurements for 0, 22.5, 45, 90,..., 337.5
' degrees. The subroutine finds the two EEPROM measurements that the current
' angle measurement falls between. It then updates the angle measurement
' based on where the angle measurement falls between the two known table values.
Compass_Interpolate:
' Start with the lowest value in the CompassCal table.
READ CompassLowVal, index
' Load current and previous table values.
READ CompassCal + index, table(current)
READ (CompassCal + (index - 1 & $F)), table(previous)
' The IF...ELSEIF...ELSE...ENDIF code block finds the two EEPROM CompassCal
' table values that the current angle measurement falls between and calculates
' the difference between the current angle measurement and the lower of the
' two table values. The IF and ELSEIF blocks deal with values that are
' greater than the highest or less than the lowest table values. The ELSE
' block everything between the highest and lowest table values.
IF (angle >= table(previous)) THEN
span = (255 - table(previous)) + table(current)
angleOffset = angle - table(previous)
ELSEIF (angle <= table(current)) THEN
span = table(current) + (255 - table(previous))
angleOffset = angle + (255 - table(previous))
ELSE
index = index - 1
READ CompassCal + index, table(current)
DO
table(previous) = table(current)
index = index + 1
READ CompassCal + index, table(current)
IF (angle <= table(current)) AND (angle > table(previous)) THEN
span = table(current) - table(previous)
angleOffset = angle - table(previous)
EXIT
ENDIF
LOOP
ENDIF
' After the offset between the current angle measurement and the next lower
' table measurement has been determined, this code block uses it along with
' the span between the table entries above and below the angle measurement
' to solve for: angle(corrected) = angle(offset) * 16 / span.
' This code block also rounds up or down by comparing the remainder of
' the angleOffset / span division to the value of (span / 2).
angleOffset = angleOffset * 16
angle = (angleOffset / span) + ((angleOffset // span) / (span / 2))
angle = ((index - 1 & $F) * 16) + angle
angle = angle & $ff
RETURN