Tracking System using 9dof Razor IMU and Servo

Right now, I'm looking into using Firmata to do this.

Don't. As has been at least hinted at, develop your own protocol. Using C++, you can write anything to the serial port. So, write something like "<n, pos>", where n is the servo to move and pos is the new position.

Then, on the Arduino, use some code like this:

#define SOP '<'
#define EOP '>'

bool started = false;
bool ended = false;

char inData[80];
byte index;

void setup()
{
   Serial.begin(57600);
   // Other stuff...
}

void loop()
{
  // Read all serial data available, as fast as possible
  while(Serial.available() > 0)
  {
    char inChar = Serial.read();
    if(inChar == SOP)
    {
       index = 0;
       inData[index] = '\0';
       started = true;
       ended = false;
    }
    else if(inChar == EOP)
    {
       ended = true;
       break;
    }
    else
    {
      if(index < 79)
      {
        inData[index] = inChar;
        index++;
        inData[index] = '\0';
      }
    }
  }

  // We are here either because all pending serial
  // data has been read OR because an end of
  // packet marker arrived. Which is it?
  if(started && ended)
  {
    // The end of packet marker arrived. Process the packet

    // Reset for the next packet
    started = false;
    ended = false;
    index = 0;
    inData[index] = '\0';
  }
}

Where it says "Process the packet", use strtok() and atoi() to get the servo number and servo position. Then, figure out how a servo number relates to a Servo instance, and set that instance's position.