How can I automatically identify and remove a fundamental frequency and all of its harmonics?
Your method isn't that bad for a first attempt.

However, the following method tends to work better:


Search for local maxima
Check for maxima that are close (2 or 3 bin spacing) and merge them
Create a few hypotheses about the fundamental frequency. You currently assume the highest peak is the fundamental frequency, which is one hypothesis. You should also check for the possibility that the highest peak is the first harmonic, i.e. that there's a smaller peak at half the frequency of the main peak. You may also have other cases to consider, using your knowledge of the problem at hand (Interference? Irregular heartbeat?).
Assuming each of those hypothesis, find the ground frequency by fitting parabola to each harmonic peak. Each peak will produce a slightly different estimate due to noise, but these errors are uncorrelated and average out. One of the hypotheses will lead to much better fits, pick the ground frequency predicted by this hypothesis.
Using the ground frequency you found in step 4 as a given, refit the parabola around each peak to estimate the height of the peak. Note that the peaks probably fall between two bins.
You now have the location and strength of the fundamental and its harmonics, but not the phase. It's probably easiest to find the phase of the fundamental, subtract that, find the phase of the first harmonic, etc.


The core reason that this works better is step 4. Any wrong hypothesis for a ground frequency will fail horribly as you're trying to fit peaks around the location of predicted harmonics. Say you've got a peak at 2 Hz. This could be the ground frequency or a first harmonic. When you test the "first harmonic hypothesis", i.e. whether the ground frequency is actually 1 Hz, you fit parabola's to the data around 1,2,3,4,5 ... Hz. If that hypothesis is wrong, you get garbage around 1,3,5 Hz. If it's right, you might find peaks near 1.1 Hz, 2.2, 3.3, 4.4 and 5.6 - which suggests that the actual ground frequency is 1.12 Hz.