Made setVel work for multiple axies
Reduced startupAxis's length
Converted kalman for vectors, now in own function