Fixed Complimentary filter

Complimentary filter fix for scaled velocity and position terms.