Signal Processing

[pepijndevos]

With robots, you have to deal with inexact inputs and outputs. For outputs, I found PID really useful, but for inputs, I don't even know what to google for.

Take for example the ultrasonic sensor. I noticed that especially with small objects or at a short distance, there are a lot of erroneous readings. if you'd draw a graph, they'd be easy to spot by hand, but how would the NXT detect these? I can come up with some crude averaging, but I suspect a few decades of automation must have produced something more sophisticated.

Going further down this path, imagine a robot that remembers its approximate heading and location, drawing a contour of its surroundings. Of course, without some sort of compass, it will drift off. I'm thinking it must be possible to recognize certain patterns in its surrounding to calibrate itself. Again, I could come up with some crude brute force comparison loop, but I suspect there are better ways out there.

What are some good things to read and algorithms to know?

[HaWe]

what I already use is a median filter (sometimes also a moving average) e.g. for compass, gyro, accelerometer, or ultrasonic sensor (median or mean of 3 or 5).
what I also use is sort of a threshold filter (don't know how else to call it in English: for a combination of compass and gyro to drop unlikely big leaps of readings, comparing all changes of readings).

Sensor readings are stored to an array: Adding a new value to the array you use one of the ArrayPush functions, for general Median of integer use ArrayMedianI (I for integer), for median of 3 integers use Median3 (array length is fixed to 3)

Code: Select all

inline long ArrayMedianI(int src[], int len)
{
  int temp[];

  ArraySort(temp, src, NA, NA)
  return temp[(len-1)/2];
}

//--------------------------------------------


inline long  Median3(int src[])
{
  int temp[];

  ArraySort(temp, src, 0, 3)
  return temp[1];
}

//--------------------------------------------

inline void ArrayPushI(long &src[], long _new, int len)
{
  long tmp[];
  ArraySubset(tmp, src, 1, NA);
  ArrayBuild(src, _new, tmp);
  ArrayInit(tmp, 0, 0);
}


//--------------------------------------------

inline int ArrayPopI(long &src[], int len)
{
  long _zero=src[0];
  for (int i=0; i<len; ++i) {src[i]=src[i+1];} // shift down
  return _zero;
}

//--------------------------------------------

inline void ArrayPushF(float &src[], float _new, int len)
{
  float tmp[];
  ArraySubset(tmp, src, 1, NA);
  ArrayBuild(src, _new, tmp);
  ArrayInit(tmp, 0, 0);
}

//--------------------------------------------

inline float ArrayPopF(float &src[], int len)
{
  float _zero=src[0];
  for (int i=0; i<len; ++i) {src[i]=src[i+1];} // shift down
  return _zero;
}

What I liked to have very much is a Kalman filter or a Particle filter. I already tried this but I had to realize that both stochastic filters will only be advantageous if there is also a external absolute reading (like absolute bearings) to compare with the relative readings e.g. of gyro, accelerometer, or compass. Attempts to get them working have been unavailing yet.

[wrutiser]

Current robotics depends heavily on probability. Lots and lots of applications of Baye's rule.

Some suggested sources:
Look through the videos at ai-class.com
Look into the courses at udacity.com
Seek lecture notes, class outlines, etc. for robotics and AI university courses.

Some jargon:
Processing data from sensors is called filtering. Kallman filters and particle filters are good examples.
Combining data from multiple sensors is called "sensor fusion".
Using sensor data and knowledge of landmarks to determine a robots position is called "localization".
Using sensor data to learn out the environment is called "mapping".
Doing both at the same time is called "Simultaneous Localization and Mapping" or SLAM.

Sebastion Thrun is a prominent robotics "rock star".

Google and enjoy.

Bill

[HaWe]

interesting what I'm already all doing although never having heard of it ;)

[aswin0]

I write a lot about sensor calibration, sensor filtering and sensor fusing on my blog http://nxttime.wordpress.com

entering these area's requires the use of simple statistics. Localization and mapping requires serious statistics. Be warned!

Btw, statistics can be fun.

[HaWe]

I've been reading your blog several times but I must admit that I didn't understand your model in order to construct a Kalman Filter for e.g., HT Gyro, HT Compass, HT Accelerometer, and odometry. It's very hard to see through what you are actually doing in your code.
The biggest problem was to get rid of unpredictable gyro drift and highly noisy accelerometer readings by using just the Kalman filter (and no additional filter models).
As I didn't understand your Kalman model I tried it with the following approach but it didn't work anyway - either there was no drift but large errors at quick rotations or there was some accuracy in rotation measurement but a big drift at standstill (code originally by Ketaljen, later modified by me):

Code: Select all

#define OFFSET_SAMPLES 100
float gOffset;

/*
 *Struktur mit DKF Parametern
 *A und H nicht benötigt da skalar und 1, somit neutrales Element
 */
struct uskf {
  float x;
  float P;
  float R;
  float Q;
};

uskf kf;

void getGyroOffset()
{
  float gSum;
  int  i, g;

  do {
    gSum = 0.0;
    for (i=0; i<OFFSET_SAMPLES; i++) {
      g = SensorHTGyro(IN_1);
      gSum += g;
      Wait(5);
    }
    gOffset = gSum/OFFSET_SAMPLES;
  } while (gOffset > 1);   // Reject and sample again if range too large

}

void dkf(int z)
{
  float K=0;   
  // statisches Modell angenommen daher kein a priori Zustandsupdate
  // a priori der Zustandsvarianz
  kf.P = kf.P +kf.Q ;  
  // Kalman Gain
  K = kf.P*(1/(kf.P+kf.R));  
  // a posteriori Zustand
  kf.x = kf.x + K*(z-kf.x);  
  // a posteriori Zustandsvarianz
  kf.P = (1 - K)*kf.P;
}


task main()
{
  int g;
  float winkel=0,winkelKF=0;
  float tInterval=0,tCalcStart=0;
 
  kf.x=0;  // Anfangsvarianz des Zustands
  kf.P=1;  // Messrauschvarianz mit 1cm angenommen(Schätzung da kein Datenblatt)
  kf.R=0.001;
  // Prozessrauschvarianz ebenfalls(Abtastzeit nicht bekannt)
  kf.Q=0.001;

  SetSensorHTGyro(IN_1);

  getGyroOffset();
 
  Wait(50);

  while(true){
    ClearScreen();
    tCalcStart = CurrentTick();
    g = SensorHTGyro(IN_1);
    dkf(g-gOffset);
    winkelKF = winkelKF + (kf.x)*(tInterval/1000);
    winkel = winkel + (g-gOffset)*(tInterval/1000);
    NumOut(10, LCD_LINE3, kf.x);
    NumOut(10, LCD_LINE4, winkel);
    NumOut(10, LCD_LINE5, winkelKF);
    Wait(3);
    tInterval = CurrentTick() - tCalcStart;
  }
}

so designing a Kalman filter for the HT Gyro was not possible neither by your approach (very hard to understand) nor by ketaljen's and mine (though setting it up quite systematically, but not working either).

[aswin0]

Hi Doc,

You're right about my implementation of the Kalman filter. It does not have gyro offset modeled in its state, nor does it utilize an accelerometer or control input that would enable it to do so over the XY-axes. Therefore it is not able to correct for this.

I abandoned the the idea of using a Kalman filter for some some time now. Instead I'm using a Non Complementary Linear filter. This filter is much simpler to understand and implement. It gives very good results as well. The main reason abandoning the Kalman filter is that the true advantages of this filter lie in the possibility to dynamically model Gaussian sensor noise. I cannot model this information as it should describe the noise within the dynamic environment of a robot, so it is not a constant you get from the datasheet. Without proper modelling of sensor noise one can just as well use a simpler filter like the NLC filter (or a lowpass filter as we'll see later) to get the same results.
The NLC filter that I currently implemented has gyro offset in its state. So it is able to correct for gyro drift over the XY-axes when attaching a accelerometer and over the Z-axis when attaching a 3-axis compass. With all three sensors attached it delivers a drift free signal with a noise band width of 0.2-0.3 degrees in a static environment.

As for your example. It doesn't model gyro offset in its state. As it declares state as a scalar it can only hold one thing, being rotational speed over the Z-axis. So, also this implementation is not able to correct for drift in gyro offset. What is more, one would need at least one other source of information, being control and/or sensor input, to correct for gyro drift if it were part of the state. So what your example does is basically to guess rotational speed from historic information (previous iterations) and correct this guess using sensor information. It therefore is nothing different than a low pass filter. Remember that a low pass filter in pseudo code is something like this

Code: Select all

filtered_signal=(1-alpha) * filtered_signal + alpha * raw_signal;

The filtered signal on the right side of the equation is the result from the same formula in the previous iteration. Alpha in this formula is a constant, the smaller it is, the more noise will be filtered out, but the filtered signal will respond slower to real changes in the signal.

To further explore the similarities and differences between the simplified Kalman filter from your example and a low pass filter. The alpha in the formula above corresponds to the Kalman gain in your filter. The filtered_signal on the right hand side of the equation corresponds to the predict phase in your filter. There is one difference though. The value of alpha is a manually set constant value in a low pass filter. In your filter the Kalman gain is calculated from the noise level of the sensor. But as you had to guess the noise level, it is not any better than guessing an alpha value directly. In a proper implementation of a Kalman filter the Kalman gain is calculated from history and true noise levels from the sensor and thus statistically sound. But if the noise level used in the filter is a constant value from the datasheet the Kalman gain would still diverge slowly to a constant and the filter would still behave like a low pass filter. Only if one is able to set the noise level dynamically, so that it would express the estimated noise at a given moment in time in a dynamic environment, only then the Kalman gain would not be a constant. And only then one would benefit from the advantages of a Kalman filter.
Well, that brings me back to the reason why I abandoned the Kalman filter. I cannot model noise dynamically, so therefore I could just as well use a much simpler filter.

It took me a year to figure this out So it will probably take another few years before I can explain this any better. Sorry for that.

[HaWe]

aswin, thank you for your explanations! Now I understand your code much better.
I abandoned the Kalman filter as well meanwhile - 1st, because all attempts failed, 2nd, because of the perception, that by theory it may handle only Gaussian distributed sensor noise and that is what the gyro noise just NOT is. 3rd because of the assumption that additional noise can't increase the accuracy of a reading (neither it's own nor another one) by comparing it or filtering it by a reading of unpredictable non-Gaussian behavior. It only might give far better results if one had external, absolute readings like bearings (which I really liked to have...) :(

EDIT:
I ssume that alpha in your formula has to be a float, 0<alpha<1 ?

I meanwhile am using something similar to a low pass filter (median and moving average/mean, the latter actually already is a low pass filter itself!) combined with what I have been calling a "threshold filter" which eliminates statistical quite unreliable readings.

I still have a dream but must admit that NXC might not be able to be calculating fast enough for it, and also knowing that I will require absolute readings as well as for the Kalman:
a stochastic particle filter (Monte-Carlo-Filter). But I also lack knowledge so far of a mathematical algorithm for an implementation yet.

Anyway, thanks for participating!

[pepijndevos]

Information overload. Thanks. I'll definitely read the blog.

Looking at the pseudocode, I'd think doc is right. at alpha=1 you just get the raw reading, while at alpha=0 you get just the (static) filtered reading. Smart!

[pepijndevos]

Follow-up to AI-class: http://www.udacity.com/cs#373

I signed up, and will probably be applying the lessons to my robots, and blog the cool stuff to http://studl.es