Hi,
The very first thing to tend to, IMHO, is to make the update rate as stable as possible - it is very important for stability. If you haven't already seen it, here's a pretty good article on digital PID filter implementation. It's a good read even if you're not actually writing the PID filter code - just using it.

Keep at it and keep us posted!