@garwalt Simply add:
servo1.write(kalAngleX/2.0); servo2.write(kalAngleY/2.0);
Just below:
timer = micros();