Allow User Defined Heading Initial Condition

[joeyzhu00]

Allow the user to define an initial condition for heading. Most likely going to be from a calibration procedure that involves driving the vehicle forward and collecting the average heading estimate after some threshold of samples has been met. This is due to the backward differencing methodology that we have adopted for heading estimation.