Tuesday, March 02, 2010

Alpha Beta Filters

(EDIT: See also Kalman filters)

Alpha-Beta filters are fairly well explained on wikipedia, and are generally a very easy first-stop solution before heading to Kalman or particle-filter alternatives. (The next obvious step is to extend this to include acceleration (Alpha-Beta-Gamma), and to limit the error/estimates to a sensible range for your application).

Here is a small example program showing how they work, it generates output similar to this:
Ideal     position: -0.897 -0.443
Mesaured  position: -0.890 -0.421 [diff:0.029]
AlphaBeta position: -0.898 -0.442 [diff:0.001]
Total error if using raw measured: 1.522438
Total error if using a-b filter:   1.059981
Reduction in error: 69% 
C source code follows:
/** A simple alpha-beta filter example by Adrian Boeing 
    www.adrianboeing.com 
 */ 

#include <stdio.h> 
#include <stdlib.h> 
#include <math.h> 

typedef struct { 
    float alpha; //alpha value (effects x, eg pos)
    float beta; //beta value (effects v, eg vel)
    float xk_1; //current x-estimate 
    float vk_1; //current v-estimate 
} AlphaBeta; 

void InitializeAlphaBeta(float x_measured, float alpha, float beta, AlphaBeta* pab) {
    pab->xk_1 = x_measured; 
    pab->vk_1 = 0; 
    pab->alpha = alpha; 
    pab->beta = beta; 
}    

void AlphaBetaFilter(float x_measured, float dt, AlphaBeta* pab) {
    float xk_1 = pab->xk_1; 
    float vk_1 = pab->vk_1; 
    float alpha = pab->alpha; 
    float beta = pab->beta; 
    
    float xk; //current system state (ie: position)
    float vk; //derivative of system state (ie: velocity)
    float rk; //residual error 
     
    //update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dt)
    xk = xk_1 + dt * vk_1; 
    //update (estimated) velocity  
    vk = vk_1; 
    //what is our residual error (mesured - estimated) 
    rk = x_measured - xk;  
    //update our estimates given the residual error. 
    xk = xk + alpha * rk; 
    vk = vk + beta/dt * rk; 
    //finished! 
     
    //now all our "currents" become our "olds" for next time 
    pab->vk_1 = vk; 
    pab->xk_1 = xk; 
} 

double frand() { 
    return 2*((rand()/(double)RAND_MAX) - 0.5); 
} 

int main(int argc, char *argv[]) {
    AlphaBeta ab_x; 
    AlphaBeta ab_y; 
    double t; //time 
    double x,y; //ideal x-y coordinates 
    double xm,ym; //measured x-y coordinates 
    double xnoise = 0; //noise we are inserting into our system
    double ynoise = 0; 
    double m_error = 0; //total error (measured vs ideal)
    double ab_error = 0; //total error (ab filter vs ideal)
#define DT 0.1
    //intialize the AB filters 
    InitializeAlphaBeta(1,0.85,0.001,&ab_x); //x position
    InitializeAlphaBeta(0,1.27,0.009,&ab_y); //y position
    srand(0); 

    for (t = 0; t < 4; t+=DT) {
        //our 'true' position (A circle) 
        x = cos(t); 
        y = sin(t); 
        //update our simulated noise & drift 
        xnoise += frand()*0.01;
        ynoise += frand()*0.01;
        //our 'measured' position (has some noise) 
        xm = x + xnoise; 
        ym = y + ynoise; 
        //our 'filtered' position (removes some noise) 
        AlphaBetaFilter(xm,DT, &ab_x); 
        AlphaBetaFilter(ym,DT, &ab_y); 
         
        //print  
        printf("Ideal     position: %6.3f %6.3f\n",x,y); 
        printf("Mesaured  position: %6.3f %6.3f [diff:%.3f]\n",xm,ym,fabs(x-xm) + fabs(y-ym)); 
        printf("AlphaBeta position: %6.3f %6.3f [diff:%.3f]\n",ab_x.xk_1,ab_y.xk_1,fabs(x-ab_x.xk_1) + fabs(y-ab_y.xk_1)); 
         
        //update error sum (for statistics only) 
        m_error += fabs(x-xm) + fabs(y-ym); 
        ab_error += fabs(x-ab_x.xk_1) + fabs(y-ab_y.xk_1); 
    } 
    printf("Total error if using raw measured: %f\n",m_error);
    printf("Total error if using a-b filter:   %f\n",ab_error); 
    printf("Reduction in error: %d%% \n",(int)((ab_error/m_error)*100));
    return 0; 
}

4 comments:

Andreas said...

Hello Adrian,
I am trying to learn more about filters. This “filter” just looks to me like a trailing weighted average. If you have a look at the graph you see how the filtered curve just smoothes the measurement curve with a delay and a loss in amplitude, but does in no way "correct" the measurements.
If you have two independant inputs, you can fuse them with a filter to estimate the true position like a Kalman or a complimentary filter.
Can you explain me why you also "filter" velocity?
As velocity is derived from position it would in my view be sufficient to just smooth position measurements, and you will get a smoother velocity aswell.

Andreas said...

sorry, a complementary filter, of course.

sukhi said...

hi adrian,do you have the same implementation in C++ as well?? It shall be great help.

Adrian said...

sukhi: No, sorry only C, but it shouldn't take much effort to convert to C++.