filtering erroneous compass values

Hi guys,

For a while now ive been working on a "robot" to take photos for spherical panoramas (see my account for examples: Stock 360° Panoramic Images and Videos for VR and More | 360Cities). Like this: http://www.gigapan.com/cms/shop/store only not so expensive, and of course WITH the satisfaction of making it myself! :smiley:

i have it working by driving on a timer, stopping every N'th seconds to take a photo. it calculates N by getting the value of a pot mapping that to 4-21 (amount of photos in one rotation) then dividing the known time of one revolution by the amount of photos in one rotation.

the problem with this is that as batteries die, the time it takes for one rev is longer and it doesnt run long enough and gets all out of whack.

what i plan on doing, (and have given it my best crack), is to Sparkfun's LSM303 - GitHub - pololu/lsm303-arduino: Arduino library for Pololu LSM303 boards
im using the same pot configuration to get amount of photos per revolution (photonum) but it is using 360 to calculate how long to drive the motor for.

this bit works. BUT what i need is a way to filter out erroneous values from the compass. It stops driving early because it thinks that it has arrived at the correct heading.

here is my code:

void drive(int gotodeg){  //gotodeg is the degrees that it drives (turns) to
  Serial.print("driving to:");
  Serial.println(gotodeg);
  digitalWrite(motorpin, HIGH); //turn on motor
  compassread(heading);  //returns "heading" as degees 0-359
  while (heading < gotodeg || heading > (gotodeg+(gotodeg/(20/photonum)))){
    compassread();  //returns "heading" as degees 0-359
    }
  }
  digitalWrite(motorpin, LOW);  //turn off motor
}

here is my FAILED attempt to error correct:

void drive(int gotodeg){
  Serial.print("driving to:");
  Serial.println(gotodeg);
  digitalWrite(motorpin, HIGH);
  compassread();
  while (heading < gotodeg || heading > (gotodeg+(gotodeg/(20/photonum)))){
    delay(1000);
    digitalWrite(motorpin, LOW);
    delay(100);
    compassread();
    digitalWrite(motorpin, HIGH);
    if (heading < gotodeg || heading > (gotodeg+(gotodeg/(20/photonum)))){
      digitalWrite(motorpin, LOW);
      for(int t=0; t < 10; t++){
        delay(100);
        compassread();
      }
      digitalWrite(motorpin, HIGH);
    }
  }
  digitalWrite(motorpin, LOW);
  
}

any one know how i can smooth out the compass output to stop my issues?

i can post photos or video to help understand, as im sure my explanation could be better.

cheers

Gr0p3r

Someone else on this forum had a similar problem and it was suggested they take several reading and averaging out the result and using the averaged value.
Another way you could do this is to use a rotary/shaft encoder or a stepper motor

Riva:
Another way you could do this is to use a rotary/shaft encoder or a stepper motor

If there is a fixed relationship between motor revolutions and angle turned, that would be the ideal way to do it.

If you need to cope with things like wheel slip, I don't see any way to avoid reading the actual compass heading.

thanks for quick response!
Peter, yes, i will need to compensate for battery degradation and i don't have a stepper motor.

riva, thats not a bad idea actually. would it be possible to take 10 readings, sort them, take off highest and lowest values and average the remainders?
what would the code look like for this?

http://interface.khm.de/index.php/lab/experiments/rotary-positionsensor-mlx90316/

gr0p3r:
riva, thats not a bad idea actually. would it be possible to take 10 readings, sort them, take off highest and lowest values and average the remainders?

Any particular reason to remove the high/low reading before averaging? The more normal code to average all readings would look something like this

int avg = 0;
for (int x=0; x<10; x++){
    avg = avg + LSM303_Compass_Reading;
}
avg = avg / 10;

The main thing to watch is the avg variable can hold all the values without overflow before it's final division.

Any particular reason to remove the high/low reading before averaging?

yes, because, when there is an eroneous value, it would be the highest or lowest value and often by quite a bit, it would throw out the average.
check the exampe below. ive marked some vallues that are wrong.

heading:  260
heading:  280
heading:  200
heading:  230
heading:  230
heading:  190
heading:  240
heading:  210
heading:  260
heading:  0   *********************
heading:  235
heading:  250
heading:  245
heading:  255
heading:  235
heading:  250
heading:  265
heading:  255
heading:  255
heading:  270
heading:  270
heading:  270
heading:  270
heading:  275
heading:  295
heading:  280
heading:  275
heading:  280
heading:  340
heading:  335
heading:  275
heading:  280
heading:  285
heading:  285
heading:  295
heading:  265
heading:  275
heading:  275
heading:  275
heading:  275
heading:  275
heading:  270
heading:  290
heading:  220
heading:  280
heading:  220
heading:  285
heading:  250
heading:  5*********************  this one made it take a photo prematurely.
Now taking photo 1 of 4
driving to:90
heading:  310
heading:  305
heading:  315
heading:  315
heading:  315
heading:  310
heading:  320
heading:  320
heading:  325
heading:  320
heading:  325
heading:  330
heading:  325
heading:  335
heading:  330
heading:  335
heading:  340
heading:  340
heading:  340
heading:  340
heading:  340
heading:  330
heading:  345
heading:  345
heading:  340
heading:  340
heading:  335
heading:  340
heading:  335
heading:  335
heading:  340
heading:  340
heading:  340
heading:  340
heading:  345
heading:  345
heading:  350
heading:  345
heading:  355
heading:  355
heading:  355
heading:  0
heading:  0
heading:  0
heading:  5
heading:  0
heading:  10
heading:  5
heading:  10
heading:  10
heading:  10
heading:  10
heading:  15
heading:  15
heading:  10
heading:  20 
heading:  25********************
heading:  45********************
heading:  35********************
heading:  15
heading:  15
heading:  10
heading:  10
heading:  10
heading:  20
heading:  20
heading:  20
heading:  20
heading:  25
heading:  30
heading:  30
heading:  40
heading:  40
heading:  40
heading:  35
heading:  35
heading:  35
heading:  35
heading:  30
heading:  30
heading:  35
heading:  40
heading:  40
heading:  40
heading:  40
heading:  45
heading:  55
heading:  55
heading:  75
heading:  50
heading:  55
heading:  65
heading:  70
heading:  60
heading:  70
heading:  60
heading:  55
heading:  60
heading:  50
heading:  55
heading:  60
heading:  65
heading:  60
heading:  60
heading:  65
heading:  65
heading:  55
heading:  65
heading:  65
heading:  90 *************************

an average would work, but i think it would work better if i could lop off the highs and lows.

riva,
Ive implemented the averages and removing the highs n lows by :

void compassread(){
  int headmax = 0;
  int headmin = 0;
  int avg = 0;

  for (int i=0; i<10; i++) {
   delay(10);
   compass.read();
   heading = compass.heading((LSM303::vector){0,-1,0});

   // record the maximum sensor value
   if (heading > headmax) headmax = heading;

   // record the minimum sensor value
   if (heading < headmin) headmin = heading;

   avg=avg+heading;
     Serial.println(heading);
  }
  heading = (avg - headmin - headmax)/8;
  Serial.println();
  Serial.println(heading);
  Serial.println();
  compassround(heading);
  Serial.write("heading:  ");
  Serial.println(heading);
  delay(100);
}

i thought i had it working but it is giving me stuff like this:

330  **ten numbers
330
329
329
329
321
319
319
323
323

365  ** avgerage (or so it thinks!)

The problem is your initializing headmin to zero so none of your readings are below this value and it never changes. In your example the 10 readings=3252 so subtracting headmax (330) and headmin (0) = 2922 and dividing this by 8 = 365. Just initialize headmin to something like 360 and it should work.

DOH! thankyou, so simple.
great. almost there! now, i have found another problem. when it gets to 355 - 5 it may get values like 355,357,0,356,3,4,0,359,358,2 giving an average of 179 - UH OH. and i am seeing this with spastic results around north.
any ideas how to overcome this?

gr0p3r:
great. almost there! now, i have found another problem. when it gets to 355 - 5 it may get values like 355,357,0,356,3,4,0,359,358,2 giving an average of 179 - UH OH. and i am seeing this with spastic results around north.
any ideas how to overcome this?

If your still removing the largest and smallest reading then the average for your example should be 143 and not 179.
Maybe adding 180 to heading if its > 180 difference from headmax and then at end using mod as result could be > 360

// record the maximum sensor value
if (heading > headmax) headmax = heading;
if ((headmax - heading) > 180) heading += 180;
---
heading = (avg - headmin - headmax)/8;
heading = heading % 360

genius!
so simple, i dunno why i couldnt think of that.

now another issue has arisen. Seams like each ttime one hurdle is overcome another one pops up.
its not stopping when the correct value is passed to it.

the error lies in this one line i belive:
while (heading < gotodeg  || heading > gotomax){
from this routine:

void drive(int gotodeg){
  int gotomax;
  Serial.print("driving to:");
  Serial.print(gotodeg);
  Serial.print( " or ");
  if (gotodeg+10 > 360) gotomax = gotodeg+10-360;
  else gotomax = gotodeg+10;
  Serial.println((gotomax));
  digitalWrite(motorpin, HIGH);
  compassread();
  while (heading < gotodeg  || heading > gotomax){
    compassread();
    }
  digitalWrite(motorpin, LOW);
  
}

i dont think the OR || is working. if i take that bit out, the first half works.

Hi gr0p3r,

I'm at work so no arduino to test on. Not related to the problem but I think you could replace the below lines of code

if (gotodeg+10 > 360) gotomax = gotodeg+10-360;
  else gotomax = gotodeg+10;

with

gotomax = (gotodeg + 10) % 360;

I assume heading gets updated by call to compassread and motor only turns pan head clockwise.
I think while (heading < gotodeg || heading > gotomax){ should be while (heading < gotodeg && heading > gotomax){

you know, i was only just reading about %mod before you posted that. Now that i understand i think you are correct with
gotomax = (gotodeg + 10) % 360;
ill give it a whirl.

I think while (heading < gotodeg || heading > gotomax){ should be while (heading < gotodeg && heading > gotomax){

im not sure thats right.
lets say put some numbers into it.
say im at 90 deg and the next photo is at 180.

while (90 < 180 (true) OR 90 > 190 (false))  = total value = "true" so loops

motor drives a bit and heading changes to 185.

while (185 < 180 (false) OR 185 > 190 (false))  =  total value = "false" so loop ends

if it was &&

while (90 < 180 (true) AND 90 > 190 (false))  =  total value = "false" so loop ends

or is my logic all arse about?

so i want it to drive if the value is less than 180 but stop if it goes too far also.

Hi gr0p3r,

Sorry for the delay in replying but when working, it's long hours so don't have energy of time to play when I get home.
Your logic is sound. I did not have the arduino here with me yesterday at work so could not test any code so I had written a simple VB6 test and it worked with AND (hence the idea) but I bought the arduino with me today and testing shows it should be OR but written like this...
while ((heading < gotodeg) || (heading > gotomax)){

The test sketch

void setup(){
    Serial.begin(9600);
}

void loop(){
    int gotodeg = 10;
    int gotomax = (gotodeg + 10) % 360;
    Serial.print("driving to:");
    Serial.print(gotodeg);
    Serial.print( " or ");
    Serial.println((gotomax));
    int heading = 270;
    while ((heading < gotodeg)  || (heading > gotomax)){
        heading++;
        heading = heading % 360;
        Serial.println(heading);
    }
    Serial.print("Arrived");
    while(1){
    }

}

nah, the delay inst a problem hey. i can only play after work anyway. only a few hours a day at most.
Ive made the changes you suggest, and right you are.
I had to add in this bit tho:

gotodeg = gotodeg % 360;

(see i used the % thing you taught me :p)
cos it was trying to drive to 360 or 10, which itll never get to.

can you also please look at:

for(int motorrun = (360/photonum); motorrun <= (360+(360/photonum)); motorrun = motorrun + (360/photonum)){
       takephoto(map(motorrun, 0, 360, 0, photonum));
       drive(motorrun); 
     }

it doesnt seem to want to stop.
its part of this loop:

    for(i = 0; i < 180; i += 30){
     tilt.write(i);
     Serial.print("Tilting to degrees: ");
     Serial.println(i);
     for(int motorrun = (360/photonum); motorrun <= (360+(360/photonum)); motorrun = motorrun + (360/photonum)){
       takephoto(map(motorrun, 0, 360, 0, photonum));
       drive(motorrun); 
     }
     drive(0);
    }

say i set photonum to 4, after a complete rotation, it doesnt tilt, just keeps on going.
output:

Tilting to degrees: 0
Now taking photo 1 of 4
driving to:90 or 100
heading: blah

Now taking photo 2 of 4
driving to:180 or 190
heading: blah

Now taking photo 3 of 4
driving to:270 or 280
heading: blah

Now taking photo 4 of 4
driving to:0 or 10
heading: blah
                                    *****  should tilt in here to 30 degrees
Now taking photo 5 of 4   *****  should reset back to 1
driving to:90 or 100
heading: blah

Hi gr0p3r,

I knocked up this test based on your supplied loop.

void setup() {
    // put your setup code here, to run once:
    Serial.begin(9600);
}

void loop() {
    // put your main code here, to run repeatedly: 
    int photonum = 10;
    Serial.println();
    Serial.print("photonum = ");
    Serial.println(photonum);
    for(int i = 0; i < 180; i += 30){
        Serial.print("Tilting to degrees: ");
        Serial.println(i);
        for(int motorrun = (360/photonum); motorrun < (360+(360/photonum)); motorrun = motorrun + (360/photonum)){
            Serial.print("motorrun = ");
            Serial.print(motorrun);
            Serial.print(",  map(motorrun, 0, 360, 0, photonum) = ");
            int x =map(motorrun, 0, 360, 0, photonum);
            Serial.println(x);
        }
        Serial.println("Tilt");
    }
    Serial.println("Exit");

    while(1){
    }
}

I altered this line
for(int motorrun = (360/photonum); motorrun <= (360+(360/photonum)); motorrun = motorrun + (360/photonum)){
changing the <= to just < and get this result

Tilting to degrees: 0
motorrun = 90,  map(motorrun, 0, 360, 0, photonum) = 1
motorrun = 180,  map(motorrun, 0, 360, 0, photonum) = 2
motorrun = 270,  map(motorrun, 0, 360, 0, photonum) = 3
motorrun = 360,  map(motorrun, 0, 360, 0, photonum) = 4
Tilt
Tilting to degrees: 30
motorrun = 90,  map(motorrun, 0, 360, 0, photonum) = 1
motorrun = 180,  map(motorrun, 0, 360, 0, photonum) = 2
motorrun = 270,  map(motorrun, 0, 360, 0, photonum) = 3
motorrun = 360,  map(motorrun, 0, 360, 0, photonum) = 4
Tilt
Tilting to degrees: 60
motorrun = 90,  map(motorrun, 0, 360, 0, photonum) = 1
motorrun = 180,  map(motorrun, 0, 360, 0, photonum) = 2
motorrun = 270,  map(motorrun, 0, 360, 0, photonum) = 3
motorrun = 360,  map(motorrun, 0, 360, 0, photonum) = 4
Tilt
Tilting to degrees: 90
motorrun = 90,  map(motorrun, 0, 360, 0, photonum) = 1
motorrun = 180,  map(motorrun, 0, 360, 0, photonum) = 2
motorrun = 270,  map(motorrun, 0, 360, 0, photonum) = 3
motorrun = 360,  map(motorrun, 0, 360, 0, photonum) = 4
Tilt
Tilting to degrees: 120
motorrun = 90,  map(motorrun, 0, 360, 0, photonum) = 1
motorrun = 180,  map(motorrun, 0, 360, 0, photonum) = 2
motorrun = 270,  map(motorrun, 0, 360, 0, photonum) = 3
motorrun = 360,  map(motorrun, 0, 360, 0, photonum) = 4
Tilt
Tilting to degrees: 150
motorrun = 90,  map(motorrun, 0, 360, 0, photonum) = 1
motorrun = 180,  map(motorrun, 0, 360, 0, photonum) = 2
motorrun = 270,  map(motorrun, 0, 360, 0, photonum) = 3
motorrun = 360,  map(motorrun, 0, 360, 0, photonum) = 4
Tilt
Exit

I think this is what you was after as it now only takes the 4 pictures and not 5.

Looks like it. Great thanks,

Ill change it when i get home and post the results. :smiley:

yay! thanks so much Riva. its all working now. ill post pics if your interested in seeing what you helped troubleshoot?

gr0p3r:
yay! thanks so much Riva. its all working now. ill post pics if your interested in seeing what you helped troubleshoot?

Glad to help. It would be great to see some pictures.