alpha_beta.c

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

int main()
{
    float dt = 0.5;
    float prev_x = 0, prev_v = 0, a = 0.85, b = 0.005;

    float input_x, state_v, diff_x;

    while( 1 )
    {
	// input signal
	input_x = rand() % 100;

	// estimate the current system state
	state_x = prev_x + ( prev_v * dt );
	state_v = prev_v;

	// find the prediction error
	error_x = input_x - state_x;

	// if alpha value between 1-0 is a knob saying
	// if we should trust the input value or the
	// estimated state.
	// 
	// 0 - trust estimated value
	// 1 - trust input value
	//
	state_x += a * error_x;

	// tweak the velocity to account for the 
	// estimation error.
	state_v += ( b * error_x ) / dt;

	// save the estimated state for the next iteration
	prev_x = state_x;
	prev_v = state_v;

	printf("%f \t %f\n", input_x, state_x);
	sleep(1);
    }
}