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