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

Advertisements