Go Down

Topic: Complementary filter along with Hanning filter (Read 2986 times) previous topic - next topic

briantee

This topic is about gyros and accelerometers used in balancing robots. I'm fairly new here so if i posted this in the wrong place, please move as required.

Was inspired by a couple of articles on first and second order complimentary filters. I was impressed with the effectiveness of single order filters setup to be an extremely simple bandpass. Along with that, adding a Hanning filter that takes a 3 reading average but the middle one has twice the weight as its neighbours on either side. While simple, its a second order giving great response.

If the accel and gyros aren't dead solid, responsive, and practically immune to noise it will always struggle to balance. Also, there is no loss of resolution using this method, there is no reason to have actual degrees, good responsive resolution is more important as a final off center measurement.

Hanning: http://www.robots.ox.ac.uk/~sjrob/Teaching/SP/l6.pdf

Filters: http://robottini.altervista.org/kalman-filter-vs-complementary-filter

Simple code of the function that would replace the Kalman filter:

Code: [Select]

float tau=0.075;            //http://robottini.altervista.org/kalman-filter-vs-complementary-filter
float a=0.0;

float han[3] = {0,0,0};
float hanAvg = 0;

float Complementary(float newAngle, float newRate,int looptime)
{
  float dtC = float(looptime)/1000.0;                                   
  a=tau/(tau+dtC) ;
  x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);

  //Hanning filter          //http://www.robots.ox.ac.uk/~sjrob/Teaching/SP/l6.pdf
  han[2] = han[1];
  han[1] = han[0];
  han[0] = x_angleC;

  hanAvg = han[0] + 2*han[1] + han[2];

  return hanAvg;




I'll be the first to admit, i don't understand the math of the Kalman filter. Cutting and pasting code without understanding it for me has never been a good way to program. The math in this is very simple-ish, easy to adjust and for a balancing robot very good bandwidth yet removing noise.

Any comments would be great. Thx Kas and many others for all your work, the credit goes to you for helping so many of us understand this stuff.

The code below is just the Module 5 from the old Balancing Robot for Dummies thread modified.
http://forum.arduino.cc/index.php?topic=8871.0

Uncomment the analogRef if using external Vref. I use Due so not required.

Code: [Select]

// Main module   K_bot angle    angles in Quids, 10 bit ADC -----------------------------
// 5 - angle and rate calculation       display ACC_Angle and GYRO_rate

//http://forum.arduino.cc/index.php?topic=8871.60

#include <math.h>

#define   GYR_Y                 0                              // Gyro Y (IMU pin #4)
#define   ACC_Z                 1                              // Acc  Z (IMU pin #7)
#define   ACC_X                 2                              // Acc  X (IMU pin #9)

int   STD_LOOP_TIME  =          9;           

int sensorValue[3]  = { 0, 0, 0};
int sensorZero[3]  = { 0, 0, 0 };
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
int ACC_angle;
int GYRO_rate;

float accZ=0;
float accY=0;
float x1=0;
float why1=0;
float x2=0;
float y2=0;

float x_angle=0;
float x_angleC=0;
float x_angleRG=0;
float  x_angleRA=0;
float x_angle2C=0;

void setup() {
// analogReference(EXTERNAL);                                            // Aref 3.3V
 Serial.begin(115200);
 delay(100);                                               
 calibrateSensors();
}

void loop() {
// ********************* Sensor aquisition & filtering *******************
 updateSensors();
 ACC_angle = getAccAngle();                                 // in Quids 
 GYRO_rate = getGyroRate();                                 // in Quids per seconds
 x_angle = Complementary(ACC_angle, GYRO_rate, lastLoopTime);

// ********************* print Debug info *************************************
 serialOut_sensor();

// *********************** loop timing control **************************
 lastLoopUsefulTime = millis()-loopStartTime;
 if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
 lastLoopTime = millis() - loopStartTime;
 loopStartTime = millis();
}
//**************************** Comp Filter ******************************************

//
float tau=0.075;            //http://robottini.altervista.org/kalman-filter-vs-complementary-filter
float a=0.0;

float han[3] = {0,0,0};
float hanAvg = 0;

float Complementary(float newAngle, float newRate,int looptime)
{
  float dtC = float(looptime)/1000.0;                                   
  a=tau/(tau+dtC) ;
  x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);

  //Hanning filter          //http://www.robots.ox.ac.uk/~sjrob/Teaching/SP/l6.pdf
  han[2] = han[1];
  han[1] = han[0];
  han[0] = x_angleC;

  hanAvg = han[0] + 2*han[1] + han[2];

  return hanAvg;
 
}


void serialOut_sensor() {
static int skip=0;
 if(skip++==5) {                                                       
   skip = 0;
 
   Serial.print(lastLoopTime);          Serial.print("\t");
   Serial.print(sensorValue[2]);          Serial.print("\t");
   Serial.print(ACC_angle);          Serial.print("\t");
   
   Serial.print(sensorValue[0]);          Serial.print("\t");
   Serial.print(GYRO_rate);          Serial.print("\t");
 
  int x = int(x_angle);
  Serial.println(x);
 
 }
}

// Sensors Module   -----------------------------------------------------------------------------------

void calibrateSensors() {                                       // Set zero sensor values
 long v;
 for(int n=0; n<3; n++) {
   v = 0;
   for(int i=0; i<50; i++)       v += readSensor(n);
   sensorZero[n] = v/50;
 }                                                            //(618 - 413)/2 = 102.5    330/3.3 = x/1024
 sensorZero[ACC_Z] -= 100;    //102;                          // Sensor: horizontal, upward
}

void updateSensors() {                                         // data acquisition
 long v;
 for(int n=0; n<3; n++) {
   v = 0;
   for(int i=0; i<5; i++)       v += readSensor(n);
   sensorValue[n] = v/5 - sensorZero[n];
 }
}

int readSensor(int channel){
 return (analogRead(channel));
}

int getGyroRate() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
 return int(sensorValue[GYR_Y] * 4.583333333);                 // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}

int getAccAngle() {                     
 return arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256;    // in Quid: 1024/(2*PI))
}

int arctan2(int y, int x) {                                    // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
  int coeff_1 = 128;                                          // angle in Quids (1024 Quids=360°)
  int coeff_2 = 3*coeff_1;
  float abs_y = abs(y)+1e-10;
  float r, angle;
 
  if (x >= 0) {
    r = (x - abs_y) / (x + abs_y);
    angle = coeff_1 - coeff_1 * r;
  }  else {
    r = (x + abs_y) / (abs_y - x);
    angle = coeff_2 - coeff_1 * r;
  }
  if (y < 0)      return int(-angle);
  else            return int(angle);
}

jremington

#1
Jan 31, 2016, 02:24 am Last Edit: Jan 31, 2016, 02:24 am by jremington
You should be aware that there are a number of so-called "Kalman filters" proposed for use in balancing robots that are just plain wrong and don't work well or at all. The example used by the robottini site is one of them, and another (which is a derivative of the first) is described here.

The problems with these misguided attempts are explained in this post and a working alternative is presented.

The math for a Kalman filter is not that difficult and is very well presented in the textbook "Optimal State Estimation" by Dan Simon.

briantee

#2
Jan 31, 2016, 03:40 pm Last Edit: Jan 31, 2016, 04:01 pm by briantee
I made a video of a step response to the accelerometer using the Kalman code in the article posted and the complimentary way. The yellow is the Kalman and the green is the "wrong" way with complimentary+hanning.

https://youtu.be/-unam2IlFHg

The Kalman overshoots so which of the constants do i change so there is no longer an overshoot with the kalman on both the initiation of a move and a stop?

I've seen other implementations that use weighting like 2% of the accelerometer and 98% of the gyro reading. Its 1 line of code, and works very well:

angle = (float)(gyro_weight * gyro_angle) + (accel_weight * accel_angle);

https://youtu.be/RcQTZr16kkI

schematic:

jremington

Quote
The Kalman overshoots so which of the constants do i change so there is no longer an overshoot with the kalman on both the initiation of a move and a stop?
You will need to rewrite the filter so that it actually works.

briantee

It is rewritten, see first post.

I have no idea how to rewrite the Kalman, i will never understand it, other then cut and paste like everyone else does that has a seriously overdamped default function that "looks" pretty. if you 1/3  the   Q_angle, Q_gyro, R_angle in the Kalman, it looks exactly like the complimentary filter and is no longer damped.

jremington

Quote
i will never understand it, other then cut and paste like everyone else does
With that attitude, you should not expect much!

People who want to learn, and actually get something useful done do not just "cut and paste".

briantee

#6
Jan 31, 2016, 11:33 pm Last Edit: Jan 31, 2016, 11:55 pm by briantee
:) You are exactly right!

On the Kalman, not sure why so many of the examples and code use the numbers they do for Q_angle, Q_gyro, R_angle.

Q_angle affects the overshoot. Was 0.003 now .031

Q_gyro - not much effect left as is.

R_angle affects rise and fall times - how closely it follows the data was .03 now 0.05

Also included the Hanning filter with the Kalman, works very nice.

charliesixpack

#7
Feb 01, 2016, 12:56 am Last Edit: Feb 01, 2016, 01:06 am by charliesixpack
You do not need any special noise filtering for a gyro like the L3G4200D.  Attached is the raw gyro output of a balancing robot.  Incidentally, the motor input on the graph is to the same Sabertooth 2X25 unit in your schematic.

The fluctuations in the gyro readings are not noise but shaking of the robot body.  You can see the shaking in the movie.


briantee

#8
Feb 01, 2016, 05:38 pm Last Edit: Feb 01, 2016, 05:55 pm by briantee
Why does it oscillate? Overshoot? Noise initially?

BTW that chip has a digital filter in it. It may use FIR, Hanning or any host of other ways. Whether its done outside or inside, there is still noise generated from any gyro/accel. Also phase shift can always be a concern in designing a filter.

A great site on filtering. http://www.dspguide.com/filtexam.htm

Does anyone have any filtering techniques they have successfully used?

MarkT

Kalman filtering relies on a correct input for the matrix of parameters, and estimating that accurately
is much harder than writing a simple filter.  However when you get it right it has optimal performance
because of the state estimation magic.
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

Go Up