Skip to main content

### 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; } ```