I wrote this program to help me understand how
localization error would propagate through the swarm. 
I intended to experiment with different methods of
data fusion, how to reconcile or average disparate
location information.

The program's usefulness is very limited as it is, but
you can see the error propagation by using the
predefined robot locations (press 'p'), then scan
('s'), and transmit ('t').  This causes all of the
robots to identify their nearest neighbors, and then
all of them transmit all of this information to
everyone else.  Use a scan radius ('S') of about 20. 
The funny triangles show the robot's actual locations
(global), and the circles show the perceived locations
(as perceived by the robot at the crosshairs).

You can place the robots in random locations ('r')
instead of predefined.  When this places a new robot
it gives it a random location, but it leaves existing
robots in the same location as before.  So, if you
want to randomize all the robot locations, specify 0
robots, then do the 'r' command again specifying
however many you want.  You can also add ('+') or
subtract ('-') robots one at a time.

The robot at the crosshairs is the robot whose
perspective is indicated by the red circles.  You can
cycle through the robots one at a time by pressing
'n'.  Pressing 'l' will display a list of the
perceived locations of the robots according to which
robot is doing the perceiving.  The 'd' key will
display global coordinates of each robot with their
heading in radians.  The amount of random error can be
controlled with 'e'.  Enter angular error and
coordinate error.  Finally, you can press 'q' to quit.

That's about it.  It's a pretty simple program.  Find
attached the display header file that draws letters
and numbers in the graphics window.  OpenGL is
supposed to be portable, so theoretically it should be
fairly straightforward to convert this program to
compile under Linux.  Feel free to post the source
code on your site with a GPL, if you're so inclined.

v/r
Rod

// 7-segment display panel code
#include <vector>
//#include "converter.h"
using namespace std;

#define VERTEXA glVertex3f(d_xorigin,d_yorigin+10.0*d_inc,d_z);
#define VERTEXB glVertex3f(d_xorigin+5.0*d_inc,d_yorigin+10.0*d_inc,d_z);
#define VERTEXC glVertex3f(d_xorigin,d_yorigin+5.0*d_inc,d_z);
#define VERTEXD glVertex3f(d_xorigin+5.0*d_inc,d_yorigin+5.0*d_inc,d_z);
#define VERTEXE glVertex3f(d_xorigin,d_yorigin,d_z);
#define VERTEXF glVertex3f(d_xorigin+5.0*d_inc,d_yorigin,d_z);

double d_inc = 0.035;
double d_z = .02;

class Display{
public:
	void init(double x, double y, double sizefactor, int digits) {
		xs.resize(0);
		sizeincrement = sizefactor;
		leftextreme = x;
		bottomextreme = y;
		dbottom = y + 3.0 * sizeincrement;
		topextreme = y + 16.0 * sizeincrement;
		rightextreme = x + 3.0 * sizeincrement + digits * 8.0 * sizeincrement;
		for(int i=0; i<digits; i++)
		{
			xs.push_back(x + 3.0 * sizefactor + i * 8.0 * sizefactor);
			strng.push_back(' ');
		}
	}
	Display(double x, double y, double sizefactor, int digits)
	{
		init(x,y,sizefactor,digits);
	}
	void drawbackground()
	{
	  glBegin(GL_TRIANGLE_FAN);
	  glVertex3f(leftextreme,bottomextreme,d_z/2.0);
	  glVertex3f(leftextreme,topextreme,d_z/2.0);
	  glVertex3f(rightextreme,topextreme,d_z/2.0);
	  glVertex3f(rightextreme,bottomextreme,d_z/2.0);
	  glEnd();
	}
	double next_right_adjacent()
	{
		return rightextreme;
	}
	double next_top_adjacent()
	{
		return topextreme;
	}
	double next_right_spaced()
	{
		return rightextreme + 3.0 * sizeincrement;
	}
	double next_top_spaced()
	{
		return topextreme + 3.0 * sizeincrement;
	}
	void draw_0(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_LOOP);
	  VERTEXA VERTEXB VERTEXF VERTEXE
	  glEnd();
	}
	void draw_1(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINES);
	  VERTEXB VERTEXF
	  glEnd();
	}
	void draw_2(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXA VERTEXB VERTEXD VERTEXC VERTEXE VERTEXF
	  glEnd();
	}
	void draw_3(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXA VERTEXB VERTEXD VERTEXF VERTEXE
	  glEnd();
	  glBegin(GL_LINES);
	  VERTEXD VERTEXC
	  glEnd();
	}
	void draw_4(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXA VERTEXC VERTEXD VERTEXB
	  glEnd();
	  glBegin(GL_LINES);
	  VERTEXD VERTEXF
	  glEnd();
	}
	void draw_5(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXB VERTEXA VERTEXC VERTEXD VERTEXF VERTEXE
	  glEnd();
	}
	void draw_6(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXB VERTEXA VERTEXC VERTEXE VERTEXF VERTEXD VERTEXC
	  glEnd();
	}
	void draw_7(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXA VERTEXB VERTEXF
	  glEnd();
	}
	void draw_8(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_LOOP);
	  VERTEXA VERTEXB VERTEXF VERTEXE
	  glEnd();
	  glBegin(GL_LINES);
	  VERTEXC VERTEXD
	  glEnd();
	}
	void draw_9(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINES);
	  VERTEXD VERTEXF
	  glEnd();
	  glBegin(GL_LINE_LOOP);
	  VERTEXD VERTEXC VERTEXA VERTEXB
	  glEnd();
	}
	void draw_neg(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINES);
	  VERTEXC VERTEXD
	  glEnd();
	}
	void draw_x(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINES);
	  VERTEXA VERTEXF VERTEXE VERTEXB
	  glEnd();
	}
	void draw_y(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINES);
	  VERTEXA VERTEXD VERTEXB VERTEXD VERTEXD VERTEXF
	  glEnd();
	}
	void draw_a(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXE VERTEXC VERTEXA VERTEXB VERTEXD VERTEXF
	  glEnd();
	  glBegin(GL_LINES);
	  VERTEXC VERTEXD
	  glEnd();
	}
	void draw_b(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXA VERTEXC VERTEXE VERTEXF VERTEXD VERTEXC
	  glEnd();
	}
	void draw_c(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXB VERTEXA VERTEXE VERTEXF
	  glEnd();
	}
	void draw_d(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXD VERTEXC VERTEXE VERTEXF VERTEXB
	  glEnd();
	}
	void draw_r(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_LINE_STRIP);
	  VERTEXE VERTEXC VERTEXD
	  glEnd();
	}
	void draw_dot(double d_xorigin,double d_yorigin)
	{
	  glBegin(GL_POINTS);
	  VERTEXF
	  glEnd();
	}
	void clr(char clrchar)
	{
		for(int i=0; i<xs.size(); i++)
		{
			strng.at(i) = clrchar;
		}
	}
	void storechar(char in,int loc)
	{
		strng.at(loc) = in;
	}
/*	void storedouble(double in,int len)
	{
		vector <char> temp = double_to_string(in,len);
		int i=0;
		while( i < temp.size() && i < strng.size() )
		{
			strng.at(i) = temp.at(i);
			i++;
		}
	}
*/	void show()
	{
	    glColor3f(0.9,0.9,1.0);
//		drawbackground();
	    glColor3f(0.0,0.0,1.0);
		for(int i=0; i<xs.size(); i++)
		{
			dispchar(strng.at(i),xs.at(i));
		}
	}
	void dispchar(char digit, double xloc)
	{
		switch(digit)
		{
		case '0':
			draw_0(xloc,dbottom); break;
		case '1':
			draw_1(xloc,dbottom); break;
		case '2':
			draw_2(xloc,dbottom); break;
		case '3':
			draw_3(xloc,dbottom); break;
		case '4':
			draw_4(xloc,dbottom); break;
		case '5':
			draw_5(xloc,dbottom); break;
		case '6':
			draw_6(xloc,dbottom); break;
		case '7':
			draw_7(xloc,dbottom); break;
		case '8':
			draw_8(xloc,dbottom); break;
		case '9':
			draw_9(xloc,dbottom); break;
		case '-':
			draw_neg(xloc,dbottom); break;
		case 'x':
			draw_x(xloc,dbottom); break;
		case 'y':
			draw_y(xloc,dbottom); break;
		case '.':
			draw_dot(xloc,dbottom); break;
		case 'a':
			draw_a(xloc,dbottom); break;
		case 'b':
			draw_b(xloc,dbottom); break;
		case 'c':
			draw_c(xloc,dbottom); break;
		case 'd':
			draw_d(xloc,dbottom); break;
		case 'r':
			draw_r(xloc,dbottom); break;
		default:
			break;
		}
	}
	void displaystuff() // delete after rewriting
	{
		double startx = -9.9;
		double starty = -9.9;
	  glColor3f(0.9,0.9,1.0);
		//drawbackground(startx,starty);
	  glColor3f(0.0,0.0,1.0);
	  startx += 3.0*d_inc;
	  starty += 3.0*d_inc;
		draw_y(startx,starty);
	  startx += 8.0*d_inc;
	  startx += 8.0*d_inc;
		draw_neg(startx,starty);
	  startx += 8.0*d_inc;
		draw_3(startx,starty);
	  startx += 8.0*d_inc;
		draw_dot(startx,starty);
	  startx += 8.0*d_inc;
		draw_5(startx,starty);
	  startx += 8.0*d_inc;
		draw_6(startx,starty);
	  startx += 8.0*d_inc;
		draw_7(startx,starty);
	  startx += 8.0*d_inc;
		draw_8(startx,starty);
	  startx += 8.0*d_inc;
		draw_9(startx,starty);
	}
	// data members
	vector <double> xs;
	vector <char> strng;
	double leftextreme;
	double bottomextreme;
	double rightextreme;
	double topextreme;
	double dbottom;
	double sizeincrement;
};