Kalman Centroid Algorithm, Swarm Robotics



I'm using this post to share some Processing code I wrote a while ago.  I was asked by David Glowacki of the Danceroom Spectroscopy project if I knew of an algorithm to determine the centre of a group of electronic handheld devices without using an external infrastructure.  I wrote a decentralised algorithm inspired by the Kalman filter but it fell short against other alternatives for the project.  I think the algorithm is interesting though, so here it is, and I'll try to explain it.  I've not checked if a similar algorithm already exists, so I'd welcome any commentary.  If you're really interested, you can play with the processing sketch, code at the end, or via GitHub.





Context

We were interested in whether a handheld device could know if it was at the centre of a group without being told it was.  Therefore, there would be no overhead tracking equipment and no big computer to dictate where the centre of a group is.  For matters of cost and an unknown environment, I decided that sensor devices like GPS, LIDAR, depth cameras, etc. were out of the question, and instead the devices should only talk to each other handheld devices using some kind of short range infra-red or radio.  As a general rule, computing is cheap, software is cheap, but useful sensors are quite expensive.

In my experience, communication devices make for poor proximity or location sensors.  However, we can extract some other useful data from them.  Namely, how many connections (or messages) are made within a given time frame.  If a device is receiving lots of messages, we could guess that it is within the centre of the group.  Importantly, we are not necessarily interested in where the devices are within a space, but in relation to each other.  The important trick here is to make sure that that the devices can only communicate within a limited range.  If all devices can communicate with each other all the time, then there would be no way to tell which devices is connected to which, and where the inside and outside of the group is.  This idea of short range communication is one of the foundations of swarm robotics and self-organising biology.

The Algorithm

At the time I was also watching some tutorial videos on the Kalman filter, this one in particular, which formed the basis of my approach:


At about 7m52s Student Dave boils the Kalman filter down to a simple equation:

Estimated State = Predicted State + difference between Actual Measurement and the Predicted Measurement 

This formula gives us four components which we can rename to our communication based centroid problem.  Since we are using how many messages are received within a time frame as our measurement, so we will call this data the message frequency:

  • Estimated State : Centroid Probability: At any point in time, we want a device to work out if it is at the centroid of the group and do something (change shape, change colour, make noises, tell the human, ...).  This is the result of our equation.  Keep in mind that it is derived from the various message frequencies below, so is itself an indication of message frequency.  
  • Predicted State : Predicted Message Frequency (PrMF): Our device can record previous values to generate a history, and use this to form a prediction of the future state.  To make a prediction, there must be a record of previous states.  In code, PrMF is created by using a windowed average, meaning that a limited number of recent values are kept to create an average, and older values are eventually discarded.  
  • Actual Measurement: Actual Message Frequency (AcMF): At any time, a device can count how many messages it has just received, giving an 'actual' reading of how connected it is to other devices.
  • Predicted Measurement: Received Message Frequency (ReMF): This forth value is needed as a comparison (the difference in Student Dave's equation).  Since we are communicating with other devices at short range, we might as well communicate something useful, and so receive their own readings of message frequency, and use these values as a 'predicted' value to balance our own.  To create this value in code, we receive messages from neighbours and average the value.  I've coded the simulation so that each device has a maximum number of messages it can receive within one time step.  

This means we can re-write the equation to:
Centroid Probability = PrMF +  K*( AcMF - ReMF )

Hopefully you spotted the 'K' in there.  Student Dave calls this the gain, and sometimes the idea of gain can seem arbitrary.  The equation above is essentially a feedback equation, where the difference in message frequency between one device (AcMF) and it's neighbours message frequencies ( ReMF) is being used to adjust a prediction (PrMF).  'K' is therefore a number used to scale, or change the influence, of the difference applied to the prediction.

If 'K' equals zero, then whatever the value of the difference, multiply it by 0 and it equals 0, and will not change the prediction.  If 'K' is a positive value, it amplifies the prediction (PrMF) by adding to it, whilst if it is a negative value it dampens the prediction (PrMF) by subtracting from it.  So in our case, 'K' will alter the influence of receiving neighbouring devices message frequency values, and so changes how much the handheld devices can inform each other.

Why would we use gain?  Gain is used when the performance of a system can vary.  In our case, the handheld devices could move around quickly, meaning that they would change their neighbours often and also change their position within the group.  The devices might be trapped within a small space and always encounter other devices, or move within a big space and only encounter other devices occasionally.  The gain allows for some calibration.

So what?  

It is quite easy to play with the simulation and find a calibration that seems reasonably good at finding the centroid.  The devices move around randomly, trapped within the window.  How the devices would actually move under human control is unknown, and probably depends a lot on what the devices do to influence the human.  In simulation, the 'a' and 'z' keys can be used to increase or decrease the number of devices.  The 's' and 'x' key can be used to change the range at which the devices can communicate. The 'd' and 'c' keys can be used to increase or decrease the gain 'k_influence'.

At any point, a device an be clicked to highlight it in white and a graph will appear to show the history of Centroid Probability for that device.


To make things more interesting, the 'e' key can be used to enable or disable an extra behaviour, which links the speed of movement of the devices to their predicted centroid value, such that they slow down if they believe they are more centroid.  The 'r' key can be used to reset the simulation.  Using the movement speed helps to discover the effect of changing the gain 'k'.


With the movement speed enabled, a positive 'k' value seems to act like glue, so that the devices will clump up into groups and remain so.




With the movement speed enabled and a negative 'k' value, the devices reduce movement in sweeping and periodic oscillations, clumping up and breaking free:



There are lots of variables to play with, such as the window size (amount of space), the number of devices (the density, related to the amount of space), as well the communication range, movement characteristics and algorithm gain.

Code

The following code has been written for Processing and you should be able to copy and paste it with no other dependencies.


  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
// Top level parameters of this program
// Can be user adjusted, see Input tab.
final int MAX_MSGS = 8;  // The maximum messages one robot can receive per cycle
int MAX_ROBOTS = 20;     // The maximum number of robots on screen
int MSG_RADIUS = 100;    // The maximum radius of communication per robot
float K_INFLUENCE = 0.2;   // The control to influence k_centroid feedback from other robots.
int HISTORY_SIZE = 25;  // The size of the windowed average, or the history of received centroid  values 
boolean RESET_POS = false;  // will reset, randomise the robots
boolean ENABLE_SC = false; // will enable speed control based on the crowd measure

// Used to draw a graph
// when a robot is selected.
Graph_t graph;

// Flexible means to add robots
// to the simulator
ArrayList Robots;


// Set up the screen
// add robots to the simulation.
void setup() {
  int i;
  
  size(600,400);
  frameRate(25);
  Robots = new ArrayList();
  
  // Add Robots to our simulation.
  for( i = 0; i < MAX_ROBOTS; i++ ) {
    Robots.add( new Robot_t() );  
  }
} 

// Main update loop
void draw() {
  int i,j;
  
  // Move 0,0 to the center of the screen
  // 0,0 is normally the top left.
  translate( width /2, height/2);
  
  // clear the previous 
  background(51);
  
  
  if(  RESET_POS ) {
     for( i = 0; i < Robots.size(); i++ ) {
       Robot_t r = (Robot_t)Robots.get(i); 
       r.Randomise();
     }
     RESET_POS = false; 
  }
  
  // Make sure robots won't overlap,
  // which also sets their new movement
  // vector
  updateCollisions();
  
  
  // do robot movement.
  for( i = 0; i < Robots.size(); i++ ) {
     Robot_t r = (Robot_t)Robots.get(i); 
     r.Move();
  } 
  
  // Do the Centroid algorithm for each
  // robot.
  for( i = 0; i < Robots.size(); i++ ) {
     
     Robot_t r = (Robot_t)Robots.get(i);
     
     r.Communicate( i );    // receive messages, exclude self (i)
     r.DoKalmanCentroid();  // run the algorithm
     r.DrawMessageRange();  // draw the message range
     
     // If selected, update and draw the graph
     if( r.clicked ) {
        graph.push( r.k_centroid.p_centroid );
        graph.draw();
     }
  }
 
  // Lastly draw each robot to the screen     
  for( i = 0; i < Robots.size(); i++ ) {
     Robot_t r = (Robot_t)Robots.get(i);
     r.Draw();
  } 
  
  // Display variables at top of sceen
  stroke( 255 );
  fill(255);
  text("K influence: " + nf(K_INFLUENCE,1,3), (width/2)-200, -(height/2) +18);
  
}

// A routine to iterate through each robot and check
// if they are colliding.  If so, the source robot is
// relocated out of infringment.
void updateCollisions() {
  int i, j;
  boolean collision;
  float d;
  float a;
  
    // We have to loop through all other 
    // robots to see if they are in collision
    // with us.  We check the distance as soon
    // as possible to minimise the time spent
    // in this loop.
    for ( i = 0; i < Robots.size(); i++ ) {
      
      // (s)ource robot.
      Robot_t s = (Robot_t)Robots.get(i);
      collision = false;
      
      for ( j = 0; j < Robots.size(); j++ ) {
        if ( j != i ) {
          
          // (t)arget robot to check against.
          Robot_t t = (Robot_t)Robots.get(j);

          // find distance between objects.
          d = dist( s.body.x, s.body.y, t.body.x, t.body.y );
          d -= (s.body.r + t.body.r);

          // If d < 0, robots are overlapping.
          if ( d < 0 ) {
            
            // find angle between objects
            a = atan2( t.body.x - s.body.x, t.body.y - s.body.y );

            // translate s by angle and distance, seperating robots
            s.body.x += d * sin(a);
            s.body.y += d * cos(a);
            
            // Flag a collision
            collision = true;
          }
          
          // We check we are not at the edges of the screen
          if( s.body.x - s.body.r < 0 - width/2 ) {
               s.body.x = 0 - width/2 + s.body.r;
               collision = true;
          }
          if( s.body.x + s.body.r > width/2 ) {
               s.body.x = width/2 - s.body.r ;
               collision = true;
          }
          if( s.body.y - s.body.r < 0 - height/2 ) {
               s.body.y = 0 - height/2 + s.body.r;
               collision = true;
          }
          if( s.body.y + s.body.r > height/2 ) {
               s.body.y = height/2 - s.body.r ;
               collision = true;
          }
          
          if( collision == true ) {
            // Since we collided, we rotate rougly 180
            s.dir += random( PI - PI/2, PI + (PI/2));
          }
        }
      }
    } 
}

//
// Class used to define objects on 2d screen
//
class Entity_t {

  float x;  // x on screen
  float y;  // y on screen
  float r;  // radius of object
  float vx; // velocity x
  float vy; // velocity u
  
  Entity_t( ) { 
    x = random( -width/2, width/2);
    y = random( -height/2, height/2);
    r = 10;
  }
  
  Entity_t( float in_x, float in_y ) {
     x = in_x;
     y = in_y;
  }
  
  
}

// Draws a simple graph.
class Graph_t {
   int w;            // width of graph on screen.
   float h;          // height of graph on screen.
   float [] values;  // stored values (over time)
   int max_values;   // maximum size of above array
   int index;        // to keep track of latest entry
   int subdivision;  // how many points per graph size = array size
   
   Graph_t() {
      int i;
      
      // Initialise
      index = 0;
      w = 300;
      h = 100;
      subdivision = 2;
      max_values = w / subdivision;
      
      // Create and clear array
      values = new float[ max_values ];
      for( i = 0; i < max_values; i++ ) {
        values[i] = 0;
      }
      
   } 
   
   // Routine so we can carelessly add new
   // data points.
   void push( float in ) {
       values[ index ] = in;
       index++;
       if( index >= max_values ) index = 0;
   }
   
   // Draws on to the top left of the window.
   void draw() {
       int i;
       int x;
       float y;
       
       // Index marks the latest entry, 
       // +1 is the most historic.
       // Its a circular buffer.
       i = index + 1;
       
       // Wrap i if we are at the end of
       // the array.
       if( i >= max_values ) i = 0;
       
       // We want to draw to the top left 
       // of the screen.
       pushMatrix();
       translate( -width/2 + 10, -height/2 + 10);
       
       // Draw background of graph
       fill( 0 );
       stroke( 0 );
       rect( 0,0,w,h);
       
       // draw data points.
       stroke(255);
       x = 0;
       
       fill(255);
       text("1",0,10);
       text("0",0,h);
       
       // We loop from i back around to
       // index, putting the points in time
       // series.
       while( i != index ) {
           y = map( values[i], 0, 1, h-1, 1 );
           point( x, y );
           x+=2;
           i++;
           if( i >= max_values ) i = 0;
       }
 
       // Return co-ordinate frame.
       popMatrix();
   }
}

// Encapsulate the IR message system so it is
// easier to manage.
class IRMsg_t {
  
  ArrayList msgs;
  int total; 
  
  IRMsg_t() {
    total = 0;
    msgs = new ArrayList();  
  }
  
  boolean underLimit( ) {
     if( msgs.size() < MAX_MSGS ) return true;
    
     return false; 
  }
  boolean push( float value ) {
     if( msgs.size() < MAX_MSGS ) {
       msgs.add( value );
       total++;
       return true;
     }
     return false;
     
  }
  int howMany () {
    return total; 
  }

  float pop( ) {
    float value;
     
    if( msgs.size() == 0 )  return -1;
   
    value = (Float)msgs.get( msgs.size() -1 );
    msgs.remove( msgs.size() -1 );
    return value;   
  }
  boolean hasMsg() {
    if( msgs.size() > 0 ) return true;
    return false; 
  }
  
  void flush() {
    int i;
    for( i = msgs.size() -1; i >= 0; i-- ) {
       msgs.remove(i); 
    }
    total = 0;
     
  }
  

}
// User input to program

void keyPressed() {

  // To Add robots.
  if ( key == 'a' || key == 'A' ) {
    if ( Robots.size() < 100 ) Robots.add( new Robot_t() );
  } else if ( key == 'z' || key == 'Z' ) {
    if ( Robots.size() > 2 ) Robots.remove(0);
  }

  // To Change Message Radius.
  if ( key == 's' || key == 'S' ) {
    if ( MSG_RADIUS < 400 ) MSG_RADIUS += 10;
  } else if ( key == 'x' || key == 'X' ) {
    if ( MSG_RADIUS > 10 ) MSG_RADIUS -= 5;
  }
  MSG_RADIUS = constrain( MSG_RADIUS, 40, 400 );

  // To Change the influence factor for the K_centroid algorithm.
  if ( key == 'd' || key == 'D' ) {
    if ( K_INFLUENCE < 1 ) K_INFLUENCE += 0.05;
  } else if ( key == 'c' || key == 'C' ) {
    if ( K_INFLUENCE > -1 ) K_INFLUENCE -= 0.05;
  }
  K_INFLUENCE = constrain( K_INFLUENCE, -1, +1 );
  
  if( key == 'r' || key == 'R' ) {
     RESET_POS = true; 
  }
  if( key == 'e' || key == 'E' ) {
     ENABLE_SC = !ENABLE_SC; 
  }
  
}


// If a mouse button is pressed and released we
// check if the user was clicking on a robot.
// Robots should never overlap, so this is a 
// quick and easy check.
void mouseReleased() {
  int i; 
  for ( i = 0; i < Robots.size (); i++ ) {
    Robot_t r = (Robot_t)Robots.get(i);
    
    // Need to compensate for the translate() in
    // the main draw loop
    // Take into account the size of the robot
    if (    (mouseX - width/2) < (r.body.x + r.body.r) 
         && (mouseX - width/2) > (r.body.x - r.body.r ) 
         && (mouseY - height/2) < (r.body.y + r.body.r) 
         && (mouseY - height/2) > (r.body.y - r.body.r ) ) {
      r.clicked = true;
      
      // Create a new graph for this robot
      graph = new Graph_t();
    } else {
      // Not clicked.
      r.clicked = false;
    }
  }
}


class KCentroid_Algo_t {
   Moving_Ave_t  ave_msg_freq;  // Probability of being centroid
   float p_centroid;            // The output of our centroid equation
   
   KCentroid_Algo_t() {
     // Initialise values;
     ave_msg_freq = new Moving_Ave_t();
     p_centroid = 0;
   }
  
  
   // 
   float ResolvePCentroid( IRMsg_t ir_msgs) {
     float ave_recv_msg_freq;          // average received message frequency
     float predicted_msg_freq;          // average historic message frequency
     float act_msg_freq;               // number received in this instance
     float msg_count;
    
     // From youtube.com/watch?v=FkCT_LV9Syk
     // Predicted state plus the difference betweent the actual 
     // measurement and the predicted measurement, multiplied by a gain factor.
     // X_est = Predicted state + (gain *  difference( actual measurement, predicted measurement)) 
     
    
     // Each robot is transmitting it's centroid estimate
     // based on message frequency
     // We construct the average from the received
     // messages.                   
     ave_recv_msg_freq = 0;    
     msg_count = 0;            
     while( ir_msgs.hasMsg() ) {
       ave_recv_msg_freq = ave_recv_msg_freq + ir_msgs.pop(); 
       msg_count = msg_count + 1;
     } 
     if( msg_count > 0 ) { // avoid divide by zero
       
       // We take the received frequencies and
       // work out the average value.
       ave_recv_msg_freq = ave_recv_msg_freq / msg_count;
       
     } else {
       ave_recv_msg_freq = 0; 
     }
     
     if( msg_count > 0 ) {
       act_msg_freq = msg_count / MAX_MSGS;
     } else {
        act_msg_freq = 0; 
     }
     
     
     // Get the historic value from the moving
     // average
     predicted_msg_freq = ave_msg_freq.GetAve();
     
     
     ave_msg_freq.Push( act_msg_freq );
     
     
     p_centroid = predicted_msg_freq + ( K_INFLUENCE * ( act_msg_freq - ave_recv_msg_freq ) );
     p_centroid = constrain( p_centroid, 0,1);
     
     
     return p_centroid;
   }
   
}


// Creates and maintains a circular buffer
// to draw and average from.
class Moving_Ave_t {
 int win_size = HISTORY_SIZE;
 float [] samples;
 int index;
 int i;
 
 Moving_Ave_t() {
   samples = new float[ win_size ]; 
   for( i = 0; i < win_size; i++ ) samples[i] = 0;
   i = 0;
 }

 void Push( int value ) {
   samples[index] = (float)value;
   index++;
   if( index >= win_size ) index = 0;
   
 }
 void Push( float value ) {
   samples[index] = value;
   index++;
   if( index >= win_size ) index = 0;
   
 }
 
 float GetAve() {
   float sum;
   sum = 0;
   for( i = 0; i < win_size; i++ ) {
      sum += samples[i]; 
   }
   if( sum > 0 ) {
     return sum / win_size;
   } else {
     return 0; 
   }
    
 }
}

// Contains parameters and functions for the on screen
// robots.
class Robot_t {

  Entity_t         body;        // Used to move and draw the robot
  IRMsg_t          msgs;        // Communication between robots.
  KCentroid_Algo_t k_centroid;
  
  float dir;            // Vector of movement.
  float mag;            // ...
  
  float msg_out;        // message value to transmit to other robots
  
  boolean clicked;                      
  
  
  // Class initialiser
  Robot_t() {
    // Create the robots body
    body = new Entity_t();
  
    // Create the robot-robot message
    // interface
    msgs = new IRMsg_t();

    // create an instance of the algorithm
    // for this robot.
    k_centroid = new KCentroid_Algo_t();

    // Give this robot a random starting
    // movement.
    dir = radians( random(360) );
    mag = 3;
    
    // Set our initial messaging value to 0
    msg_out = 0;
    
    // default is not selected by user
    clicked = false;
  }
  
  // Routine to randmoise the robot
  // position, we just re-initialise the
  // entity class to do this.
  void Randomise() {
     body = new Entity_t(); 
  }
  
  
  void Communicate(int myself) {
    int i;
    float d;
    
    // Check this robot against all others
    for( i = 0; i < Robots.size(); i++ ) {
      Robot_t r = (Robot_t)Robots.get(i);
      if( i != myself ) {
        
        // See if we are in range 
        d = dist( body.x, body.y, r.body.x, r.body.y );
        if(  d < MSG_RADIUS ) {
          
          // Receive from the other robot.
          // Check if message queue is full
          // Note: if a robot can receive more
          // messages it will get a clearer reading,
          // but it depends on how many robots there are 
          // within a given space (density vs connectivity)
          if( msgs.push( r.msg_out ) ) {
            
            // draw a line to indicate successful communication
            strokeWeight(2);
            if( clicked ) {
            stroke(255, 255, 255, 150);
            } else {
               stroke( 220,100,100,80);
            } 
            line( body.x, body.y, r.body.x, r.body.y );
          }
        }
      }
    } 
  }
  
  // wrapper to call the kalman algorithm and
  // sets the message out value for this robot
  // and clears out previously received messages.
  // (Needs to happen in this order)
  void DoKalmanCentroid(){
    
    // Resolve the probability of being at the
    // centroid of a cluster.
    // Pass the algorithm all the messages received
    msg_out = k_centroid.ResolvePCentroid( msgs );
    
    // Now we clear out messages to receive a new
    // batch
    msgs.flush();
  }
  
  
  // Determines the next position of the robot
  // Simply x/y cartesian equations, based on 
  // a direction and magnitude.  Magnitude can
  // be varied in response to the kalman algorithm
  // if the 'e' key is pressed to enable it.
  // Dir is determined by the collision detection
  // function.
  // Collision detection is done in one iterative
  // pass over the whole simulation from the 
  // top level of code (see KalmanSwarmCentroid_Simple tab)
  void Move() {
    
    // Every time it moves it makes a
    // tiny deviation to it's direction
    // to be less boring.
    dir += randGaussian( 0, 0.2);
    
    if( ENABLE_SC ) {
      mag = 3 * (1-msg_out);
    } else {
       mag = 3; 
    }
    
    // after we have checked against all robots
    // and updates our direction, this robot can 
    // work out the new x/y coordinates 
    body.x += mag * cos( dir );
    body.y += mag * sin( dir );

  }

  

  void Draw() {

    float fill_col;

    // Change the colour of the body
    // based on the probability of being 
    // in the center of mass
    if( clicked ) {
      stroke(255);
    } else {
      stroke(0);
    }
    
    // Change the shading based on the 
    // centroid estimate
    fill_col = k_centroid.p_centroid;
    fill_col = map( fill_col, 0, 1,0,255 );
    fill_col = constrain( fill_col, 0, 255 );
    
    fill( 20,200,20);
    ellipse( body.x, body.y, body.r*2, body.r*2);
    
    fill( fill_col, 20, 20, fill_col );//255 - fill_col );//fill_col );
    ellipse( body.x, body.y, body.r*2, body.r*2);
    
  }
  
  void DrawMessageRange() {
     fill( 20, 20, 20, 40 );
     stroke( 20, 50, 20, 40 );
     ellipse( body.x, body.y, MSG_RADIUS*2, MSG_RADIUS*2 ); 
  }

}


// Processing seems to have removed its Random class
// in the latest versions so here is a guassian 
// random number generator.
// Source:
// http://www.taygeta.com/random/gaussian.html
float randGaussian( float mean, float sd ) {
   
   float x1, x2, w, y;
   
   do {
     x1 = (2 * random(0, 1) ) -1;
     x2 = (2 * random(0, 1) ) -1;
     w = (x1 * x1) + (x2 * x2); 
     
   } while( w >= 1.0 );
   
   w = sqrt( (-2.0 * log( w ) )/w );
   y = x1 * w;
   
   return mean + y * sd;
   
}