Fix some type stability problems and formatting

[roflmaostc]

Hi!

Thanks for the great implementation!

However, in my applications I can't achieve reasonable results in comparison to our cross correlation based methods.

While digging into the code, I found a few type instabilities. This fix should improve speed slightly for single precision objects such as Matrix{Float32}.

Would be happy to receive some feedback :)

Best,

Felix

[mileslucas]

Thanks for the thorough PR! I'm going to rerun the local benchmarks and push a new version.