Archives for category: Uncategorized

I have included a compass into my INS filter. It now is drift-free and accurate over all three axis. It also is very robust to magnetic disturbances. The compass itself is not. It gives me great pleasure to see the first results.

Tuning the filter is difficult though. I have used the NXJChartingLogger that will be part of Lejos 0.9.1 a lot. The filter provides all kinds of output that is displayed real time on the pc. Different parameters are logically grouped into different graphs. The graphs provide information on the state of the filter and thus help in tuning.
I have also included a method to introduce errors in the sensor data. This enables me to examine the performance of the filter.

As a last thing I have made the filter calculate the initial state of the filter. With this the filter settles accurately (<.5degrees) within half a second.

The performance dropped to 97 Hertz with three sensors attached. But I haven't optimized the compass code yet.

In later posts I'll demonstrate the performance of the filter.

Advertisements

Today I accomplished a big speed improvement of my IMU filter. The filter frequency used to be maximal 140 Hertz. It now is 240 Hertz.

What is the effect of this speed improvement you might wonder? Well the main thing is that one iteration of the filter takes less processing time. This leaves more processing time for other processes. Another effect is that the integration error is less. So the filter should be more accurate.

How did I realize the speed improvement? The filter uses vectors and matrices and a lot of lineair algebra. Before I had a 3×3 matrices based on a 2 dimensional array of 3×3 elements and vectors based on one dimensional arrays of 3 elements. I replaced the arrays with variables and I got rid of the loops around the arrays. So what used to be something like this:

For (int i=0;i<3;i++)
val[i]+=x;

Now has become something like this:

val0+=x;
val1+=x;
val2+=x;

It makes the code ugly and a bit harder to maintain. But who cares.

Soon to come. Compass integration in the IMU filter! This makes it a 9DOF filter and will prevent filter drift over the Z-axis.

for my IMU sensor I created a filter that fuses the readings from the gyro and the accelerometer to get drift free and low noise attitude data. You can read more about this in my previous posts. In this and the next post I give you the code for the filter.

The code consists of three parts. First there is the code to interface with the sensors. This part of the code is very specific to the type of sensors that I used. As you probably have a different gyro and accelerometer it won’t be of much use to you. Therefore I will not publish this part of the code.
The second part of the code consists of function that support matrix and vector math. This part of the code will be presented below. The third part of the code contains the filter itself. This will be the subject of my next post.

The code is written in robotC, but as far as I can see the matrix functions are not robotc specific. If you use another c dialect you should be able to modify it without much hassle. Only the two display functions use robotC specific code, you’ll have to modify this when you use another C dialect.
Some of the choices I made find their origin in using robotC. At first I implemented the library as functions, the vectors and matrices that are the input and result of matrix calculations were passed by reference. However, there seems to be a bug in robotC, once my programs got larger some of the functions behaved odd. So I had to to find another way to implement the calculations. I ended up using directives. Directives are resolved by the precompiler. They have some disadvantages, less readable code, larger compiled programs and slower execution. But they worked without problems. I will call the directives functions for readability, although technically I shouldn’t.

Some words about the conventions I used in the code. Every function is named after the calculation it implements, like sum, substract or dotproduct. In addition to the name, characters are added that supply information about the parameters to the function. Preceeding the name is one character presenting the type of the return parameter, M for matrix, V for vector and S for scalar. Trailing the name are characters representing the input parameters of the function. So VsubstractVV accepts two vectors as input and returns a vector. All parameters, both input and output are supplied as parameters to the function call. There are no real return parameters. To call the VsubstractVV function you write VsubstractVV(inputVector1, inputVector2, outputVector).

#define Msize 3
typedef float matrix[Msize][Msize];
typedef float vector[Msize];


// transforms a vector in a skew symmetric matrix 
#define MskewSymmetricV(v, m ) \
{\
  m[0][0]=0;\
  m[0][1]=-v[2];\
  m[0][2]=v[1];\
  m[1][0]=v[2];\
  m[1][1]=0;\
  m[1][2]=-v[0];\
  m[2][0]=-v[1];\
  m[2][1]=v[0];\
  m[2][2]=0;\
}


// returns the product of 2 matrices in matrix r
#define MproductMM(m1, m2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
  {\
    for ( intRcol=0;Rcol<Msize;Rcol++)\
    {\
      r[Rrow][Rcol]=0;\
      for ( int ii=0;ii<Msize;ii++)\
        r[Rrow][Rcol]=r[Rrow][Rcol]+m1[Rrow][ii]*m2[ii][Rcol];\
    }\
  }\
}

// returns the sum of 2 matrices in matrix r
#define MsumMM(m1, m2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rrow][Rcol]=m1[Rrow][Rcol]+m2[Rrow][Rcol];\
}


// sums two vectors
#define VsumVV(v1, v2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    r[Rrow]=v1[Rrow]+v2[Rrow];\
}

// substracts two vectors
#define VsubstractVV(v1, v2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    r[Rrow]=v1[Rrow]-v2[Rrow];\
}


// calculates the product of a matrix and a vector
#define VproductMV(m, v, r)\
{\
  for ( int Rrow=0;\Rrow<Msize;\Rrow++)\
  {\
    r[Rrow]=0;\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rrow]+=m[Rrow][Rcol]*v[Rcol];\
  }\
}


// returns the dot product of two vectors
#define dotProductVVS(v1, v2, dp)\
{\
    dp=0;\
    for( int ii = 0; ii < Msize; ii++)\
      dp += v1[ii]*v2[ii];\
}

// returns the cross product of two vectors
#define VcrossProductVV(v1,v2, vOut)\
{\
    vOut[0] = (v1[1]*v2[2]) - (v1[2]*v2[1]);\
    vOut[1] = (v1[2]*v2[0]) - (v1[0]*v2[2]);\
    vOut[2] = (v1[0]*v2[1]) - (v1[1]*v2[0]);\
}

// multiplies a vector with a scalar
#define VscaleVS(v,scale, vOut)\
{\
    for( int ii = 0; ii < Msize; ii++)\
      vOut[ii] = v[ii]*(scale);\
}

// returns one row of a matrix in a vector
#define VgetRowMS(m, Rrow, v)\
{\
  for ( int Rcol=0;Rcol<Msize;Rcol++)\
    v[Rcol]=m[Rrow][Rcol];\
}

// returns one column of a matrix in a vector
#define VgetColMS(m, Rcol, v)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    v[Rrow]=m[Rrow][Rcol];\
}


// Displays the values of a matrix
#define displayM(m)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      nxtDisplayStringAt(1+Rcol*33, 63 - Rrow*9, "%2.2f",m[Rrow][Rcol]);\
}

// Displays the values of a vector
#define displayV(v, start)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
	    nxtDisplayStringAt(1+start*33, 63 - (3+Rrow)*9, "%2.2f",v[Rrow]);\
}

// returns the difference of 2 matrices in matrix r
#define MsubstractMM(m1, m2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rrow][Rcol]=m1[Rrow][Rcol]-m2[Rrow][Rcol];\
}

// Transposes a matrix
#define MtransposeM(m, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rcol][Rrow]=m[Rrow][Rcol];\
}

// copies one matrix into another
#define McopyM(m, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rrow][Rcol]=m[Rrow][Rcol];\
}



// copies one vector into another
#define VcopyV(v, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    r[Rrow]=v[Rrow];\
}

// translate euler angles to direction cosine
#define Meuler2cosineV(v, m)\
{\
  m[0][0]=cos(v[1])*cos(v[2]);\
  m[0][1]=sin(v[0])*sin(v[1])*cos(v[2])-cos(v[0])*sin(v[2]);\
  m[0][2]=cos(v[0])*sin(v[1])*cos(v[2])+sin(v[0])*sin(v[2]);\
  m[1][0]=cos(v[1])*sin(v[2]);\
  m[1][1]=cos(v[0])*cos(v[2])+sin(v[0])*sin(v[1])*sin(v[2]);\
  m[1][2]=cos(v[0])*sin(v[1])*sin(v[2])-sin(v[0])*cos(v[2]);\
  m[2][0]=-sin(v[1]);\
  m[2][1]=sin(v[0])*cos(v[1]);\
  m[2][2]=cos(v[0])*cos(v[1]);\
}

// translate cosine to euler
#define Vcosine2eulerM(m, v)\
{\
  v[0]=atan2(m[2][1],m[2][2]);\
  v[1]=asin(-m[2][0]);\
  v[2]=atan2(m[1][0],m[0][0]);\
}

This blog tells you of my progress with Sidbot and the IMU sensor.

The sensor is working fine. I can get it to work with the higher baud rates RobotC offers. Just not reliable. So I have to dive into this. I really need to have the higher baudrate to run the algorhithm for balancing at high speed. I aim for 100 Hertz, but I hope 50 Herz will do.
My drivers for the sensors are working well. This I consider finished.
I made a wrong descision to include the blinkM with the IMU unit. As the now share the same port, they also share the same band width. What’s more, they also have to share the same thread as the I2C calls in robotC are not thread safe.
Getting 3D attitude information from a 3-axis gyro is not obvious. I have to dive deep into trygonometry and vector math to understand and code for the transformations required. This is because the robot and the gyro are not fixed in space, they can have any attitude. And when the robot is upside down a clock-wise turn seems counter clockwise for the sensor. You have to account for this in the code, and not just for this extreme attitude but for all possible attitudes. It is funny how my robot building projects always seem to end up in math and statistics. I can only conclude that my mind is drawn to this as I find little similar experiences on the web. What’s even more funny is that my best source of information has been a book written by a guy who’s primary field of interest were missile guidance systems. So it really is rocket science!
In the mean time I have also modified Sidbot, I have lowered the center of gravity as much as possible. This makes it more stable. It can now handle much larger accelerations. You have to remember that it must perform in tilted situations when on the ball. It gave me some quality time with my son.
Next weeks will be spent on writing an inertial navigation system (INS) for the robot. So that it will always know its attitude and position. The code will just take a few lines. I expect more problems with the stability of the system. It must be reliable/stable for a few minutes but this might not be a realistic goal. We’ll see.

I finished building my IMU sensor and I wanted to share my experience with the sensor. My experiences with building it are in a different post.

To house my sensor I used the (never used) sound sensor. I cut a prototype board so that I could slide it in the ridges that are on the inside of the housing. This keeps my sensor in the right direction. On the board I mounted the IMU and the voltage regulator. I had some trouble to to get the large LDO voltage regulator and the caps in, but in the end it worked. I even found some space to add the blinkM to the sensor. It might well be the most versatile NXT sensor around, it has a temperature sensor, 3 gyros, 3 accelerometers and a all color LED . So far the good news.

The accelerometer is quite good, although its resolution is limited to 4mg. The range is -16g to 16g. It has a small offset, too small to be corrected by the sensor itself. Funny (?) thing is the offset changes with the rate I query the sensor.

The gyro has a very small bias, for a lot of implementations one wouldn’t need to correct for it (but I do). It doesn’t work well with the higher baud rates robotc offers, so I have to query it using 9.6Kb. That is a disappointment and also a bit strange as it supports fast mode I2C.

(Both the accl offset issue and the gyro I2C issue make me think that my power supply needs improvement)

The blinkM is a lot of fun. I can advise it to everyone. It is not hard to connect to the NXT and it appeals to a lot of people, to my kids at least. I had to make a police robot with sirens and flashlights, and so I did. But the blinkM proves itself as a handy debugging tool for dynamic robots as well. To give an example. I wanted to know how fast the gyro drifts in a dynamic environment, not while standing still. I made a program that fires the blinkM bright red when having a specific attitude. I then placed my NXT and the sensor on my Technics turn table. In the beginning the blinkM fired when the NXT was facing straight to me, but I could see it drift away slowly. I didn’t dive deeper into this as there is a lot of work to do with my new sensor.

Combining the three different devices into one obviously has an advantage, you need just one sensor port. There are also disadvantages to sharing the sensor port. The first is that the different devices need to share the limited band width (it takes about 20 msec to query bot the gyro and accelerometer). Another thing is that you need to write one routine to query both. I started out with separate routines for each of the sensors but I found out that the routines collided around the sensor port, this might be robotC specific though.

All in all. I’m rather content but I think there is still some room for improvement.

This post presents the robotC code for the Kalman filter.

Now the Kalman filter is working I made the code suitable for use in robotC progams. The code can be freely used by you. Download the code

To use the filter include heading.c into your program. This will create a global variable called heading that contains the filtered heading. The variable is updated every 20 mSec. 

When you include the code into your program a task will be started automaticly once your program starts. This task will first search for both a compass and a gyro sensor. Once they are found the filter starts calculating the value of heading.  The program can find the sensors as long as they are defined with robotC’s motors and sensors setup.

Check the readme and example program called testHeading.c for additional information.

The code probably will not work when the sensors are connected through the sensor multiplexer becuse the function that searches for the sensors only scans the first four sensor ports. But in case you have a multiplexer it should be easy to modify the program.

Initially I used vector math to implement the filter in robotC.  For this version however I got rid of vector math and switched to normal type of (calculus) functions. This makes the code (and the filter) so much more easy to understand.

Here is the code, in case you just want to look at it.

//*!!Code automatically generated by 'ROBOTC' configuration wizard							 !!*//

/*************************************************************************************************\
*																																																	*
* PROGRAM: Heading    	      																																		*
* VERSION: 0																																											*
* PURPOSE: This program integrates gyro and compass readings using a Kalman filter to get a more	*
*					 accurate value for heading                     																				*
* AUTHOR:	 Aswin Bouwmeester (aswin.bouwmeester@gmail.com)																				*
* DATE:		 okt 10, 2010	  																																				*
*																																																	*
\*************************************************************************************************/

/*
variance for the gyro sensor for different sample sizes

N   T   var
1	  5	  0,222
2	  10	0,144
3	  15	0,123
4	  20	0,119
5	  25	0,114
6	  30	0,088
8	  40	0,093
10	50	0,078
*/


#pragma systemFile
#pragma autoStartTasks


// MOUNTING can be set to - if the compass or the gyro is mouned upside down
#ifndef MOUNTING
#define MOUNTING
#endif



float meanSensorValue(tSensors s, int interval, int N);
tSensors findSensor(TSensorTypes type);
float heading=-1;


task getheading()
{

	float
		var_compass = 0.9,
		var_gyro=0.119,
		var_filter_predicted,
		var_filter_updated,
		kalman_gain,
		compass_measured,
		compass_predicted,
		compass_updated,
		gyro_measured,
		time_span,
		offset;
		;

	long
		time_start,
		time_end
		;

	bool
		disturbed
		;

	tSensors
	  _compass,
	  _gyro
	  ;

		// find the compass and gyro sensors;
		_compass=findSensor(sensorI2CMindsensorsCompass);
		if (_compass==S5)
		  _compass=findSensor(sensorI2CHiTechnicCompass);
		_gyro=findSensor(sensorI2CHiTechnicGyro);

		// Display error message if the sensors are not found
		if (_compass==S5) nxtDisplayTextLine(1,"No compass found");
		if (_gyro==S5) nxtDisplayTextLine(2,"No _gyro found");
		if (_compass==S5 || _gyro==S5)
		{
		  PlaySound(soundException);
		  while(nNxtButtonPressed==-1);
		}
		else
		{

      // get the gyro offset
			offset=meanSensorValue(_gyro,5,100);

			// initialise the filter;
			compass_updated=SensorValue(_compass);
			var_filter_updated=0;

			// Run the filter forever;
			while (true)
			{
				// get time span;
				time_end=nPgmTime;
				time_span=((float)(time_end-time_start))/1000.0;
				if (time_span<=0) time_span=0.02; // this is to compensate for wrapping around the nPgmtime variable;
				time_start=nPgmTime;

				// get measurements from sensors
				// (when changing the sample size of the gyro, one must also change the variance)
				compass_measured=(float)SensorValue(_compass);
				gyro_measured=MOUNTING (meanSensorValue(_gyro,5,4)-offset);

				// predict;
				compass_predicted=compass_updated+time_span*gyro_measured;
				var_filter_predicted=var_filter_updated+var_gyro;

				// heading must be between 0 and 359
				if (compass_predicted<0) compass_predicted+=360;
	      if (compass_predicted>=360) compass_predicted-=360;

				// Detect _compass disturbance;
				if (abs(compass_predicted-compass_measured)> 2 * sqrt(var_filter_predicted))
					disturbed=true;
				else
					disturbed=false;

				// get Kalman gain;
				if (disturbed)
					kalman_gain=0;
				else
					kalman_gain=var_filter_predicted/(var_filter_predicted+var_compass);

				// update;
				compass_updated=compass_predicted+kalman_gain*(compass_measured-compass_predicted);
				var_filter_updated=var_filter_predicted+kalman_gain*(var_compass-var_filter_predicted);

				// make result available gobally
				heading=compass_updated;

				// display informatin about filter

#ifdef HEADING_DISPLAY
        nxtDisplayTextLine(0,"Heading filter");
        nxtDisplayTextLine(1,"Heading  : %3.0f",compass_updated);
        nxtDisplayTextLine(2,"Compass  : %3.0f",compass_measured);
        nxtDisplayTextLine(3,"Variance : %6f",var_filter_updated);
        nxtDisplayTextLine(4,"Disturbed: %1d  ",disturbed);
#endif
				// wait for next iteration;
				wait1Msec(0);


			}
	}
}

// function used to automaticly find sensor out in what port a sensor is attached (returns S5 when sensor is not found)
tSensors findSensor(TSensorTypes type)
	{
	  tSensors port;
	  for (port=S1;port<=S4;port++)
	    if (SensorType[port]==type) return port;
	  return S5;
	}

// get a mean sensor value from specified number of samples
float meanSensorValue(tSensors s, int interval, int N)
	{
	  float som=0;
	  for(int i=0;i<N;i++)
	  {
	    som+=SensorValue[s];
	    wait1Msec(interval);
	  }
	  som=som/N;
	  return som;
	}

In my last post I discussed how I improved the accuracy of my robots sensors. This was done by trying out different sensor locations and measuring the variance of the sensors at different locations. This time I’ll describe how I measured the variance for both the compass and gyro sensor.

Earlier I found out that the NXT motors influence the accuracy of both sensors in two ways. First, the magnetic field generated by the NXT motors influence the compass sensor. I minimized this effect by using the brick as a shield. For this I placed the compass sensor at the top rear of the brick. The NXT motors are underneath and in front of the brick. Second, changes in motor power seem to affect the voltage level on the sensor ports for a short period of time. I’m still figuring out a good method to minimise this effect. For measuring the variance of the sensors I got rid of this effect by  pausing sensor readings for a short time after changes in motor power.

For my final test I wrote a program that set motor power to a random level between -100 and 100 every 0,5 seconds. At the end of this time period, just before applying a new value for motor power, I measured the value of both sensors. I did this 300 times, taking two and a half minutes. The motor power was applied to both driving motors of FreeRover, but not to the steering motor. FreeRover was placed on a cardboard box to get the wheels free from the ground and thus to prevent FreeRover from moving. The environment was as stable as could be whilst at the same time the system could vibrate (although not as much as it would when FreeRover really moves). Below is a graph of the measurements.

 The graph shows the readings of the compass sensor in red. It is pretty stable, even with the motors running. There are a few readings of the mean giving it a variance of 0.03544. The Gyro readings are in blue, it shows more variation in the readings. The variance of the Gyro is 0,93311 in this test. I did repeat the test several times at different headings and locations. The results of the tests were not always the same but the overall image is consistent with this example.

I want to finish this post with a remarks. This isn’t a test that satisfies scientific standards, but hey, the NXT is a toy after all.

I did the experiment I wrote about in my last post. It turned out my theory only worked while driving forward. Then FreeRover could do without the steering motor. The speed difference between the two front wheels pulled the steering mechanism exactly in the right angle. But when driving backwards things didn’t turned out as I expected and hoped for. When driving backwards the steering mechanism turned the opposite way.

Why? My theory ist hat the force on each wheel has two effects. First, it makes the car move. This is shown as a green vector in the image below. As the force on the right wheel is bigger than the force on the left wheel the right wheel goes faster and the car makes a turn. This is the effect I was hoping for. Second, it makes the wheel try to rotate along it’s own steering axle (the red dot). The direction of this effect is shown by the purple vector. This effect also makes the steering mechanism rotate. As the force of the right wheel is bigger this effec

When driving forward these effects work in the same direction. When driving backward the second force workes in the opposite direction. As it easier to turn the wheel around its steering point than it is to move the car the second effect wins. As a result the steering mechanism steers the wrong way.


Is there a way to neutralise the second force? In theory there is one. If the steering axle of the wheel goes right through its centre (the red dot being at the centre of the wheel) then there is no second effect. But I think this is impossible to make.

FreeRover has each of its front wheel driven by a NXT motor. Steering is done by a third motor. The speed of the each of the driving motors is dependent of the steering angle. Last night I was wondering if FreeRover could do without a steering motor. If so, this would free one NXT motor for other purposes.

My idea is that the steering angle could be made mechanicly dependent from the (difference in) speed of both steering wheels. The opposite, the speed of both wheels being dependent of the steering angle, can be achieved mechanicly with the use of a differential. I think it can be done, and quite easily as well. If the steering angle can be set without much friction it will do so automaticly if both wheels turn at different speeds. The idea is tempting. Also, I can test it quite easily, i just have to remove one of the gears from the steering gear train. This will free the steering mechanism from the steering motor. I will have to rewrite some of the code though. Now the motor speed is derived from the current steering angle. It must be derived from the aimed steering angle. No big deal.

Are there any disadvantages in terms of stability or the ability to follow the intended path? I don’t know yet. Tests will tell me. It’s going to be a busy evening!

After a healthy holiday I came to the conclusion a low pass filter isn’t the way to go. Low pass filters are good for removing high frequency noise. But the errors the ultrasonic sensor give me aren’t like that. The errors it gives me are single misreadings occurring at random intervals (far apart). These misreadings give very high values most of the time. I’ll call these errors spikes as they look like spikes when you plot US readings graphically.

In a low pass filter these spikes are treated as valid values where I just want to ignore them. Also, in a low pass filter a spike affects the filter for some time after it occurred. This is also an unwanted effect of a low pass filter in my case.

There is a statistic called median that is not very sensitive to exceptional values. It is more or less like the mean value of a couple of readings but is calculated in a different way. The mean value is defined as the sum of all readings divided by the number of readings. The median is defined as the reading that separate the higher half from the lower half of all readings. Let me give an example. Suppose I got 5 readings from the US sensor, 100, 98, 255, 94, 92. The value of 255 is a misreading, the actual distance was 96 at that time. The median of these readings is 98, as there are 2 numbers bigger than 98 and two numbers are smaller. The median is quite close to the actual value of 96. In this case the median gives a better estimate of the average reading than the mean value (127,8).

A low pass filter is like the mean, they both assume all readings are valid although maybe not very precise on their own. The median is different in this respect, it ignores the biggest and smallest readings and assumes only that the most average readings are valid. If I want to get rid of the spikes in the US sensor readings I need a filter that behaves like the median. Luckily it is not to difficult to create a filter based on medians. The filter should just return the median of the last couple of readings form the US sensor.

So how does a median filter behave? Suppose a median filter that returns the median value of the three most recent readings and an object is approaching the US sensor. This filter would be able to completely filter out all spikes that last only one reading (unlike the low pass filter). If a spike lasts two or more readings it will not be ignored at all. The filter could still be able to ignore these situations when it uses more than three readings.
Also, the filter needs time to responds to changes in readings (just like the low pass filter). This becomes clear if an object is closing in on the US sensor. The result of the filter would lag one reading from the actual situation. A median filter based on more than three readings would suffer from even more lag. So there is a trade-off between the strength of the filter and the time it needs to adjust.

It’s good to be aware of the potential difficulties in programming a median filter. First,  one needs to keep track of the history of readings, this takes memory space and requires the use of queues, arrays or lists. Second, the readings have to be sorted by value for being able to find the median. This has to be done again for every reading. 

The US sensor updates every 50 ms, so it is of no use polling it faster.  Spikes lasting longer than one reading are rare, so taking the median of the last three readings will most often be sufficient. In this case one can even create a median filter without queues and sorting routines.

For FreeRover I implemented a median filter based on three readings and this filter works well.