I am building quadcopter with IMU to get orientation in 3D and ultrasound range finder to measure distance to the ground. For some mechanical reasons, IMU is not aligned with quadcopter axis. Range finder is pointing down perpendicular to the copter plane. IMU has left-hand coordinate system where X axis pointing down, Z axis backwards and Y to the right. IMU provides quaternion to describe the orientation. The following picture illustrates the setup.

There are two problems I am facing:

1. To get orientation of the copter, I need to adjust the quaternion from IMU.
2. To get real altitude (distance to the ground) I need to take in account the pan an tilt of the base to adjust measurement from range finder.

I am using numpy-quaternion library.
To solve the first problem (adjust IMU quaternion to get copter orientation) I placed the copter on the known horizontal surface, got 5 quaternions from IMU, average them, then create quaternion without rotations and calculate the difference quaternion as suggested here:

qs = quaternion.as_quat_array(sensor_data)
avg_imu_pos = np.average(qs)
base_q = quaternion.from_rotation_vector([[0, 0, 0]])[0]
diff = base_q * avg_imu_pos.conjugate()

Then to get copter position I am multiplying calculated difference with quaternion from IMU:

orientation_true = diff * orientation

where orientation is the quaternion received from IMU. As far as I can tell, this step is working properly. I made simple visualization and it looks correct to me.

The problem I can not solve for now is how to adjust the range finder measurement. I made the following function for it:

def correct_distance(self, measured_distance: float, orientation: quaternion) -> float:
    q1 = quaternion.from_rotation_vector([[0, 0, 0]])[0]
    diff = q1.conjugate() * orientation
    if diff.w < -1:
        diff.w = -1
    elif diff.w > 1:
        diff.w = 1
    angle = 2 * math.acos(diff.w)
    return measured_distance * math.cos(angle)

The assumption here is that orientation is the quaternion describing the orientation of the copter, i.e. corrected IMU quaternion. So the idea was to calculate the angle between not rotated quaternion and quaternion describing copter position. Then use this angle to adjust the distance measurement by multiplying it by cos(angle).

Unfortunately, this step leads to unexpected results. Even rotation around vertical axis considerably decrease the corrected measurement which is obviously wrong.

So my question is – is my general strategy correct and if yes, did you see any mistake preventing me from getting properly adjusted distance measurement?