Allow initializing with full quaternion.

[rikba]

This PR exposes initialization using the full orientation. This becomes useful when starting MSF with an IMU that is not perfectly aligned with the inertial frame. May solve https://github.com/ethz-asl/ethzasl_msf/issues/169 and others.

Remarks:

• This PR does not solve the issue that MSF is initialized from parameter server instead of the first message arriving which would be more convenient in my opinion. In our case we wrote an extra node that reads the first IMU orientation and GPS position from ROS messages and then writes it to dynamic reconfigure. I can share this node if desired.
• This PR removes initialization from yaw only. If there is a neat or preferred way to preserve backwards compatibility we can implement that. Let me know.

[ethzasl-jenkins]

Can one of the admins verify this patch?

[clanegge]

Looks good. Would maybe make sense to change it for the other sensors as well (PositionPoseSensor and SphericalPositionSensor), to stay consistent