Ultrasound Signal Processing Methods I am sending 6.6Vpp square wave across a narrow-band, 40 kHz Transmitter to a narrow-band, 40 kHz Receiver. I want to find a very accurate distance between the two (sub millimeter). 

My assumption so far is that if I can get very consistent results at a fixed distance, that I can calibrate the speed of sound and an offset error time to get consistent results (and absolute, not just relative) at a bunch of different results.  

Unfortunately, I've tried 3 different signal processing methods and none of them are yielding the perfect results I'm looking for. I'm sampling at 500 kHz.


Threshold method: I transmit 10-20 periods, and then look for the received waveform to pass a given threshold, then I take this sample to be the TOF. Obviously if there is noise, there is going to be some error, so its understandable why this doesn't work.
BFSK method: I transmit 50 periods at 41 kHZ, 20 periods to linearly transition to 39 kHz, and then 50 periods at 39 kHz. I receive the waveform, and use a zero detection algorithm, then detect instantaneous frequency. When I detect the frequency crossing at 40 kHz, I use this to determine TOF. This is my most accurate method. But I come across a strange error: sometimes I get all of my results with precision all within 0.5mm (500 trials), but othertimes I run this code, I get a BUNCH of errors, like 50% of the errors are greater than 5 mm. 
Cross correlation method: I store a previous received signal that was taken at a known distance in memory, then I correlate my received signal to this result. My correlation only uses 4-5 periods on each side of the peak of the trial signal to reduce the computation time of the correlation. 


Any recommendations on real-time, robust, sub-mm signal processing methods for ultrasound rangefinding?
Cross correlation should work.  I think the problem is the waveform that you are using.  A square wave has bad auto-correlation properties.  If it is a periodic square wave it will have multiple peaks.  It sounds like you are just using a single pulse which is better, but it will still have a gradual roll-off which is a problem.  Instead, use a Barker code, which will give you a nice sharp peak and very low side peaks.  I would BPSK modulate the barker code and then cross correlate what you receive with the "ideal" signal.