Kalman Filters


/*
Explaination of Kalman Filters
By: Zachary Sasser

A kalman filter is basically a fast way to find a real value for a sensor.
(A fast way to find averages in a way but don't mistake it as a way to find a mathematical average)

We could just take a ton of data and take the average and remove outliers and it probably would give you the true value you want.
But taking the average method usually requires a lot of data to get accurate and
on timing intervals like acceleration that means inaccuracy and timing issues.

A kalman filter can converge to a value with much less data than a simple average
and when combined with things like a low pass filter you can get pretty good accuracy
on pretty inaccurate hardware.

*/

//Use pathagorean theorum to get the magnetude of acceleration.
//If the values are negative the value will come out negative (because CS != math) and so use abs(); (absolute value)
int getAccel(){
return abs(sqrt(pow(accel_x(),2)+pow(accel_y(),2)+pow(accel_z(),2)));
}

double KG; //The kalman gain (a coefficient explained later I'm just defining it here)
double change; //You normally don't use this I'm seperating the variables to explain things easier

int main(){

//Get an inital guess at what we think the number probably is.
//You can actually set this value to pretty much anything random because the kalman filter will narrow down the value very fast anyway.
double estimation = getAccel();

//Print our initial estimate so its tangeble for us to understand
//NOTE IF YOU DON'T DO %f you'll get giant values because we are working with doubles not ints
printf("Initial Estimate: %f \n", estimation);

//This is what the error in each of our measurements probably is.
//If this value is large it will have slower performance and the error takes longer to reduce
//However if the value is too low you might get completely wrong values or you won't end up with as accurate of values
//(because the formula thinks that you're measurements are more accurate than they are)
double measurementError = 50;

//This is how much error we think is in the current estimate (in this case our initial estimate)
//It can be used as a general measurement of how much error our estimation is so far
//This value can also be pretty much anything but if it's too low you might get bad values
double estimationError = 50;

double targetError = 10;
//how much error we'd like to have in our estimate after
//(note you might want to do iterations instead of a error)

while(estimationError<targetError){

//STEP ONE: Kalman Gain
//The kalman gain is a measurement of how close to the true value we probably are.
//In other words it takes into account how much error we have compared to the error in the measurement.
//We will use this later to make sure that new values don't change the current one as much.
KG = estimationError/(measurementError+estimationError);

//STEP TWO: Update Estimation
//This is how much our estimate needs to change taking into account the new data (getAccel())
//Note as our program goes on the value of KG goes down
//If KG is small (close to zero) this change value will be almost nothing because new data shouldn't change our estimation by much
//At the beginning KG is larger (close to 1) and so the new data has a lot more pull on what our next estimation should be.

//Basically if we are already pretty certain about our guess, we don't want noise or a measurement to change our guess
//to a completely different value.
change = KG*( getAccel()-estimation);

//We add this change to our estimation to update what we think the value should be.
//NOTE: Normally you don't split up the variables like this you'd just add them in one go to preserve speed and memory.
//I split it up here to help explain that it is a CHANGE to the estimation when new data is added.
estimation = estimation + change;

//STEP THREE: Update Reported Error
//Now that our estimation should be more accurate we need to update our reported error as such.
//If KG is close to 0 then we should be at a fairly low error or high accuracy so the error wont change as much from this new data
//If KG is close to 1 then the error should change by more because the new values are updating the value a lot more
estimationError = (1-KG)*estimationError;

}

}

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google+ photo

You are commenting using your Google+ account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s