Looks like in EKF, we choose between 3-2-1 Tait-Bryan (yaw-pitch-roll) and 3-1-2 rotation based on the roll and pitch value. The comment says it is to avoid gimbal lock. However in EKF, the rotation is represented by a quaternion. Will the gimbal lock still be relevant in this case? Is it only used to get/set the “yaw” of the rotation?
// We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence
// when there is more roll than pitch tilt and a 3-1-2 rotation sequence
// when there is more pitch than roll tilt to avoid gimbal lock.
And looks like the 3-2-1 Tait-Bryan (yaw-pitch-roll) and 3-1-2 rotation matrix from the code are slightly different. Is it expected? For example, for yaw=10deg, pitch=5deg, and roll= 5deg. In 3-2-1 rotation, the rotation matrix is
# R1 = rotation_z(10deg) * rotation_y(5deg) * rotation_x(5deg)
R1 = [[0.981060262190407, -0.165506672615463, 0.100639471732756],
[0.172987393925089, 0.982379315192455, -0.0707548063739084],
[-0.0871557427476582, 0.0868240888334652, 0.992403876506104]]
And according to the code here, the 3-1-2 rotation matrix is
r = np.arcsin(R1[2, 1])
p = np.arctan2(-R1[2, 0], R1[2, 2])
# R2 = rotation_z(yaw) * rotation_x(r) * rotation_y(p)
R2 = [[0.979712728005427, -0.172992422833534, 0.101176045714131],
[0.180462869007085, 0.981088782547442, -0.0699853818258461],
[-0.0871557427476582, 0.0868240888334652, 0.992403876506104]]