Servos calibrated and correct, kalman filter still has issues