Like with any optimization, you have to try and see. Profile to find bottlenecks, analyze them and refactor or optimize your code. The algorithm itself is not necessary a problem but specifics of implementation. For example, integral part of PID controller needs an array of errors from previous measurements, so it’s a good idea to pre-alocate this array instead of resizing it dynamically.