I am building and application where I need to switch an IMU on for around 5 seconds, and get a good orientation within the first 1-2 seconds. I am using the Madwick quaternion algorithm and it works relatively well. The problems is, if the sensor is rotated about 180 degrees while it being off, it takes a long time to find this new orientation. Is there a way to initialise the quaternion values from the IMU sensor (instead of the standard 1, 0, 0, 0) to something close to the correct orientation? I am trying to do this by first calculating roll, pitch and yaw from the imu with the following:

pitch = 180 * atan2(accelX, sqrt(accelY*accelY + accelZ*accelZ))/PI;
roll = 180 * atan2(accelY, sqrt(accelX*accelX + accelZ*accelZ))/PI;

mag_x = magReadX*cos(pitch) + magReadY*sin(roll)*sin(pitch) + magReadZ*cos(roll)*sin(pitch)
mag_y = magReadY * cos(roll) - magReadZ * sin(roll)
yaw = 180 * atan2(-mag_y,mag_x)/M_PI;

and the converting these to quaternions with the following:

double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);

Quaternion q;
q.w = cr * cp * cy + sr * sp * sy;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;

However the heading seems very random. Please let me know if you know of a method to initialise the quaternions with semi-correct values.