Part three: Kalman filtering

Part three: Kalman filtering

6 – So… where are we now ??

As a reminder, here is the pseudo code for the balancing process:

loop                                        (fixed time loop 10ms = 100Hz)
- sensors aquisition, smoothing and zeroing
- Acc angle (quids) and Gyro rate (quids per second) calculation and scaling
- Acc and Gyro data fusing though Kalman algorithm                          << here we are
- PWM calculation using a PID control algorithm
- PWM feeding to DC motors
endLoop

7 – Data fusing

So, what do we get sofar ?? a noisy Acc angle and a drifting Gyro rate of rotation
Let’s fuse them !!!
I have tried 2 filtering technologies:

- Complementary filter
This filter is rather easy to understand
I have seen several successfull implementation using complementary filters
I tried it and got mixed results, probably due to some non related design flaws
More details here: http://web.mit.edu/scolton/www/filter.pdf

- Kalman filter
Well over normal understanding Shocked Shocked , but… efficient
Please see http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
more details at http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/15 (Reply #24)
I modified a code snippet I found on the Web to produce an easy to use/self contained function
see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0 (Reply #14)

When rotating the board, make sure that GYRO_rate has the right sign:
GYRO_rate should be positive when rotating board toward +256 Quids (+180°)
and negative when rotating board toward -256 Quids (-180°)
This is another pitfall (been there, done that Wink)

The smoothing effect is quite impressive and is best viewed in graphing packages such as Processing or LabView


The red line is the raw ACC angle, the blue one is the filtered Signal

This is the updated code including Kalman filter:

// Main module   K_bot angle    angles in Quids, 10 bit ADC -----------------------------
// 7 - Data fusing, Kalman filter

// Installation:
// Create "Kalman" and "Sensors" tabs
// Cut and paste the 2 modules in their respective tab
// Save as "Kas_bot_angle"

#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 actAngle;
int ACC_angle;
int GYRO_rate;

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

void loop() {

// ********************* Sensor aquisition & filtering *******************
  updateSensors();
  ACC_angle = getAccAngle();                                           // in Quids
  GYRO_rate = getGyroRate();                                           // in Quids/seconds
  actAngle = kalmanCalculate(ACC_angle, GYRO_rate, lastLoopTime);      // calculate filtered Angle

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

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

void serialOut_labView() {
  Serial.print(actAngle + 512);        Serial.print(",");         // in Quids (0-512-1024)
  Serial.print(ACC_angle + 512);       Serial.print("\\n");
}

// --- Kalman filter module  ----------------------------------------------------------------------------

    float Q_angle  =  0.001; //0.001
    float Q_gyro   =  0.003;  //0.003
    float R_angle  =  0.03;  //0.03

    float x_angle = 0;
    float x_bias = 0;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    float dt, y, S;
    float K_0, K_1;

  float kalmanCalculate(float newAngle, float newRate,int looptime) {
    dt = float(looptime)/1000;
    x_angle += dt * (newRate - x_bias);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;

    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;

    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;

    return x_angle;
  }

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

We now have a nice filtered value reflecting the position of the bot:
Negative value when leaning forward
Positive value when forward
Zero when vertical, at equilibrium

Most of the sensitive job is done
Again, don’t go further if you don’t get an accurate, stable reading
This will save you a lot of frustration Wink

<-Back