Updated filtering coefficients

Updated and checked the critically damped and butterworth coefficients.
Created a test case for looking at the COP position. Started
experimenting with a residual analysis funtion.