Added functions to filter vectors and quaternions.
Created function and variables to filter Gyro Rates. mCurrentRates is now a filtered reading.
Changed FeedForward gains to be 3x1, as well as readInputFile() to match.
Cleaned up the trajectory generation definitions.