Filters Kalman, Vision or PID - the boat is sailing in a straight line

Can you post the code you're using now?