Implementing Sensor Fusion Algorithm Cold Start

Read 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.