Can i add orientation to imu msg handle ?

[tdnet12434]

I tried to use position_pose_sensor but not successful to estimate yaw w.r.t earth it seem to drift overtime. and i tried to add doublePose that mention https://github.com/ethz-asl/ethzasl_msf/issues/80#issuecomment-95958085 to add second pose that will give only orientation information. But not successfully because lack of knowledge about cpp template code style. But i will learn it later.

So , someone can guide me which way is easily to add orientation w.r.t. earth from imu msg? Because i have orientation that calculate from AHRS packed into imu msg. Thank you advance and sorry for my bad english :D