I partially finished developing a new software iteration based on a threaded architecture. The new software is already showing much better timing results with some segments showing more than a factor of 2 increase over the previous iteration due to not only the threading but optimizations added during the redesign.
However, the timing results indicate that the software is still not capable of providing a near real-time position calculation. A possible solution is being researched utilizing the computer's high performance clock.