Wednesday, November 5, 2014

Sensor Fusion #1 : Alpha Beta Gamma Filter

Sensors are prone to error, no matter how much cash you spend. One way is to spend a lot getting an extremely accurate sensor, while other, more favorable way is to filter out the noise and drift in the faulty reading to get an output as close as possible to the ideal case.

In line with this, let me introduce, the Hello World of Sensor fusion, Alpha Beta Gamma Filter (or Alpha Beta Filter, if not using the acceleration term)

The Alpha Beta Filter is quite an intuitive, yet a very simple concept to begin with. The best explanation that i myself found was on Wikipedia. So instead of delving further explaining the same thing again and again, let me just give you the appropriate link of Alpha Beta Gamma filter from Wikipedia. Please do read it faithfully. It is very insightful and clearly explained.

Algorithm Summary

Initialize.
  • Set the initial values of state estimates x and v, using prior information or additional measurements; otherwise, set the initial state values to zero.
  • Select values of the alpha and beta correction gains.
Update. Repeat for each time step ΔT:
  Project state estimates x and v.
  Obtain a current measurement of the output value
  Compute the residual r.
  Correct the state estimates.
  Send updated x and optionally v as the filter outputs

Now the major aspect is the implementation. I have implemented the entire filter on C++ with simulated noise and drift. The code is given below. Feel free to copy it and make sure you run it on your platform.

Edit ---- The parsing for blogspot is just something i wont appreciate a lot. You can instead find the codes in my github repository. Fork me there, to get the code. The link is just below.
harpribotics.blogspot
  





/* An alpha beta filter implementation by Harshal Priyadarshi
Blog - http://harpribotics.blogspot.com
*/

#include
#include
#include
#include

using namespace std;

struct alphaBeta{
 float alpha;  // alpha value - affects position
 float beta;   // beta value  - affects velocity
 float pos_in; // current position value 
 float v_in;   // current velocity value
};

void initiateParams(float pos_measured, float alpha, float beta,  struct alphaBeta * storeAB){
 storeAB -> alpha  = alpha;
 storeAB -> beta   = beta;
 storeAB -> pos_in = pos_measured;
 storeAB -> v_in   = 0;
}

void Filter_AB(float pos_measured, float delT, struct alphaBeta * storedAB ){
 // initialize parameters and initial position
 float alpha  = storedAB -> alpha;
 float beta   = storedAB -> beta;
 float pos_in = storedAB -> pos_in;
 float v_in   = storedAB -> v_in;
 
 // update the position
 float pos_fin;
 pos_fin = pos_in + v_in * delT;
 
 // update the velocity
 float v_fin;
 v_fin = v_in;
 
 // calculate the innovation or residual
 float innov;
 innov = pos_measured - pos_fin;
 
 // update the final position and velocity using the innovation
 pos_fin = pos_fin + alpha * innov;
 v_fin   = v_fin   + (beta/delT) * innov;
 
 /* replace the old position and velocity in storedAB
 with the new ones for next filtering*/
 storedAB -> pos_in = pos_fin;
 storedAB -> v_in   = v_fin;
  
}

// to add noise to the true value to get measured value
double generateRandom(){
 // generate random number between 0 and 1
 float random = ((float) rand()) / (float) RAND_MAX;
 // return random number between -1 and +1
 return 2*(random - 0.5);
}

int main(){
 // initialize two parameter definitions for x and y coordinates for position
 struct alphaBeta AB_x;
 struct alphaBeta AB_y;
 
 // initialize time, ideal x-y coordinates, measured x-y coordinates,
 // noise in x and y, and finally the error between measured and ideal 
 // value and filter and ideal value. Also the time intercal delT
 double t;
 double x_id, y_id;
 double x_meas, y_meas;
 double noiseX = 0, noiseY = 0;
 double measError = 0;
 double alphaBetaError = 0;
 double delT = 0.1;
 
 // now initialize the alpha beta filter
 initiateParams(1,0.85,0.005,&AB_x); //x position
    initiateParams(0,0.90,0.009,&AB_y); //y position
    
    for (t = 0; t < 4; t+=delT) {
    //our 'true' position - it will be a circle 
        x_id = cos(t); 
        y_id = sin(t); 
        //the noise progressively added to the true value to simulate drift
        noiseX += generateRandom()*0.01;
        noiseY += generateRandom()*0.01;
        //our noisy measured position 
        x_meas = x_id + noiseX; 
        y_meas = y_id + noiseY; 
        //our 'filtered' position with alpha-beta filtering 
        Filter_AB(x_meas,delT, &AB_x); 
        Filter_AB(y_meas,delT, &AB_y); 
         
        //print  the output
        printf("Ideal     position: %6.3f %6.3f\n",x_id,y_id); 
        printf("Mesaured  position: %6.3f %6.3f [diff:%.3f]\n",x_meas,y_meas,fabs(x_id-x_meas) + fabs(y_id-y_meas)); 
        printf("AlphaBeta position: %6.3f %6.3f [diff:%.3f]\n",AB_x.pos_in,AB_y.pos_in,fabs(x_id-AB_x.pos_in) + fabs(y_id-AB_y.pos_in)); 
         
        //update error sum (for statistics only) 
        measError += fabs(x_id-x_meas) + fabs(y_id-y_meas); 
        alphaBetaError += fabs(x_id-AB_x.pos_in) + fabs(y_id-AB_y.pos_in); 
    }
    printf("Total error if using raw measured value: %f\n",measError);
    printf("Total error if using a-b filter:   %f\n",alphaBetaError); 
    printf("Reduction in error: %d%% \n",(int)((measError - alphaBetaError/measError)*100));
 // hold the screen
 int ender;
 cin>>ender;
}
 
 
//Sample output generated 
//Total error if using raw measured value : 1.237430
//Total error if using a-b filter         : 0.915765
//Reduction in error: 49%
















Posted by



at





































No comments:















Post a Comment


















        

      









Subscribe to:
Post Comments (Atom)