I am working on the MRS1104C and I am trying to map an orthogonal box. I am using only 1 of the 4 layers. I have positioned the sensor perpendicular to the floor and I get the measurements of distance, angles and quaternions from the embedded to the sensor IMU. In order to fully map the box, I rotate the sensor 200 degrees relative to the vertical axis on the floor (rotation 1) and also some degrees relative to the horizontal axis (rotation 2).
In order to tranform to the global coordinate system I use the Quaternion-derived rotation matrix (Python).
x = distance * math.cos(phi) * math.cos(theta)
y = distance * math.cos(phi) * math.sin(theta)
z = distance * math.sin(phi)
a11 = 1 - (2 * (qy**2 + (qz**2)))
a12 = 2 * (qx * qy + qw * qz)
a13 = 2 * (-qw * qy + qx * qz)
a21 = 2 * (qx * qy - qw * qz)
a22 = 1 - (2 * (qx**2 + qz**2))
a23 = 2 * (qy * qz + qw * qx)
a31 = 2 * (qx * qz + qw * qy)
a32 = 2 * (-qw * qx + qy * qz)
a33 = 1 - (2 * (qx**2 + qy**2))
x_final = a11 * x + a12 * y + a13 * z
y_final = a21 * x + a22 * y + a23 * z
z_final = a31 * x + a32 * y + a33 * z
The data for the Quaternion-derived rotation matrix are obtained by the IMU of the sensor. So, new values from the sensor are received each time the sensor is moved. However, after the transformation, a different slope on the same side of the box is observed for the measurements from 0 to 137.5 degrees with respect to the measurements from -137.5 to 0 degrees, as can be seen at the attached figure. Figure of the box from measurements
I believe that this should not be observed, because each new measurement takes into account the new quaternions and thus the transformation for the same point on the same side of the box should give almost the same result.
1) Could you please give me some ideas why this happen and also, could you please give me some advices or guide me how can I solve it?
2) Am I missing something?
3) Is the above methodology correct?
Please see the figures below:
Image with colors blue is from -137.5 to 0 degrees and red from 0 to 137.5 degrees
