This is a heck of a project - very cool! I'm envious - wish I had more time to devote to something like this. My first job out of college was real-time servo control of a high-speed (~100KHz frequency) high-accuracy (.0000625-mm) vertical actuator, quite similar in operation to the THC you're developing here. That may have been my favorite job to date! I didn't have too much of and hand in the filter design, but more of the software architecture around it and higher-level algorithm work. However, I believe that a properly-tuned PID controller loop (http://en.wikipedia.org/wiki/PID_controller) could be what you're looking for, if you haven't already tried it. I'm not the guy to help tune the various coefficients (sounds like agent4573 has expertise here), but I might be able to help implement and optimize the code, if you like.

This is dangerous for me - one more thing to put on the list of cool projects that I want to work on and occupy my time. I'll be watching with great interest - keep it up!