Added initial filtering with the gyro, noise drastically reduced without transient response or drift problems, but need to properly rotate gyro into the world frame of reference