Try using an Alpha-Beta filter, see
http://en.wikipedia.org/wiki/Alpha_beta_filter
It is very simple to implement, so simple for my team I've created a simple Excel spreadsheet that takes a series a data points, the alpha and beta values, and plots the raw and filtered values in a graph. I can then adjust the alpha and beta values in Excel and view the changes in real time until I see the results I like and then plug those into the robot.
Steve