The servo control and stabilisation are pretty straight forward.
A good place to start would be having a look at the open source code of Ardupilot on DIY Drones in their version 2 software.

They are using the FMA sensors, as is nearly everyone else producing a cheap autopilot, so you might be onto something if you can do image analysis instead of using the IR sensors.
I would see that as the biggest challenge.
Although a camera in each direction would still cost a lot more than the FMA Thermopiles.