500 counts of error would be 500/20000 = 0.025 inches. I would expect you can do better. What are your requirements? Under what conditions?
You might add a 2nd order Low pass Filter as the last IIR Filter of 500Hz Q1.4. Then see if you can increase D gain and P gains for less error.
You might add some I Gain. You will want to increase the Max I to 2000 as well.