Implementing Sensor Fusion Algorithm Cold Start

I've implemented this from ideas in the FreeIMU forum. The microchip reads sensor data to calculate an approximate orientation when starting. This allows subsequent outputs to converge to the actual value much faster, instead of slowly converging in around 60 seconds. 

It appears that this is the most popular FreeIMU repository on github. So I've posted the changes here.