I am doing a project tracking a moving person's location and orientation to several points fixed in reference frame by a Hedge. Code is conducted under Python 3.5/3.7. So I build a map by 4 static Beacons for the reference frame, and using the quaternion through IMU data to compute the data I want. There are several problems that confuse me and need to be clarified by you.
1. The quaternion received by ''valuesImuData" seems to be related to the orientation at the time powering the Hedge. In other word, it treats the orientation when powering on the hedge as zero point. I did some experiment that place the hedge on table, switch on the hedge and switch off the hedge, then rotate some angle only by z axis, and switch on again. I found every time switching on the hedge, the angle to z axis is zero. It has nothing to do with the reference frame build in DashBoard. If that is the case, every time starting the hedge, it need to be align with the frame built in DashBoard to initiate body frame on hedge. I am not sure the situation above is correct. Hedge is fully calibrated according to the Manual. Angle obtained by the following code
Code: Select all
if isinstance(q, np.ndarray):
elif isinstance(q, list):
w, x, y, z = q
ysqr = y*y
t0 = +2.0 * (w * x + y*z)
t1 = +1.0 - 2.0 * (x*x + ysqr)
X = math.degrees(math.atan2(t0, t1))
t2 = +2.0 * (w*y - z*x)
t2 = 1 if t2 > 1 else t2
t2 = -1 if t2 < -1 else t2
Y = math.degrees(math.asin(t2))
t3 = +2.0 * (w * z + x*y)
t4 = +1.0 - 2.0 * (ysqr + z*z)
Z = math.degrees(math.atan2(t3, t4))
return X, Y, Z
2. During my experiment, the angle about Z axis becomes bigger and bigger at a very slow step, even I did not touch the hedge, and even I pressed the Zero IMU button several times, the situation only happens on Z axis. Can you suggest any other methods can solve this problem?
Here is the settings of hedge and a video of increasing angle
Please feel free to let me know what need to be report further more.
Thanks in advance!