question about solving the w_R_c0 in initial procedure

[cyy1992]

After refining g_c0, it should be solving w_R_c0, right? This piece of code corresponding to the formula should be like this in utility.cpp. I think R0 is the w_R_c0, right? And I am confused the left code in estimator.cpp, can you give the corresponding formulas? Thank you very much!!

[qintonguav]

The meaning behind this code is that we want to find a transformation, which can rotate the gravity to the Z-axis, meanwhile, the yaw angle is zero at the beginning.

[cyy1992]

Sorry， I have not understood yet. I know the Z-axis in world frame is alone with the gravity vector([0,0,9.8]^T), but I can't understand how the yaw angle set zero. Can you explain more clearly please? I'am sorry to bother you again!

[QMonteCarlo]

I guess this is because we do not have a geomagnetic sensor to know the absolute orientation in world frame. the best we can do is to align gravity, which fixes pitch and roll, yaw angle is irrelevant, so just set to zero is OK.

[cyy1992]

@QMonteCarlo Thank you for your reply. Can you explain how the yaw angle set zero according to the code.

[QMonteCarlo]

@yyIsMe Here is my own derivation, if anything wrong with it, please help me correct it. in line 432 with Utility::g2R(g)--> Utility::g2R R0 = Quaterniond.FromTwoVectors(ng1,ng2), R0 = Rz(yaw1)Ry(pitch1)Rx(roll1) so here R0 rotate g to z axis, the direction of g may be expressed as: \hat{g} = Rx(-roll1)Ry(-pitch1)Rz(-yaw1) \hat{z} here I simply ignores the length, \hat{z} is the normalized direction. so the returned R0 = Rz(-yaw1)*R0 = Ry(pitch1)*Rx(roll1)

Now turn to line 433,here I call double yaw as yaw2: in line 434, now R0 = Rz(-yaw2)*Ry(pitch1)Rx(roll1) operating on g: g = R0g = Rz(-yaw2)*Ry(pitch1)*Rx(roll1)*Rx(-roll1)*Ry(-pitch1)*Rz(-yaw1)\hat{z} = Rz(-yaw2-yaw1)\hat{z} note that for any angle, Rz(yaw)\hat{z} = \hat{z}, so finally g is along the z direction now, yaw2 and yaw1 does not matter. Here again I ignore the length.

As with R[0]: rot_diff * R[0] = Rz(-yaw2)(R0R[0]) // here R0 is from line 432 before multiplication = Rz(-yaw2)*(Rz(yaw2)*Ry(pitch2)*Rx(roll2)) = Ry(pitch2)*Rx(roll2) so now you see, the yaw angle of Rs[0] is now 0 as the author claimed. Here is the program ouput:

I'm glad to know any mistake on my derivation.

[cyy1992]

@QMonteCarlo Thank you very much for you answer. But I have not understood clearly about the yaw2. The formular in the program should be yaw2 = yaw(Ry(pitch1)*Rx(roll1)*Rz(yaw_Rs)*Ry(pitch_Rs)*Rxz(roll_Rs)), I still doubt it, and can you explain it? I am so sorry to bother you again!

[QMonteCarlo]

@yyIsMe I also spent some time here. This form of decomposition Ry(pitch1)*Rx(roll1)*Rz(yaw_Rs)*Ry(pitch_Rs)*Rxz(roll_Rs) = Rz(yaw2)Ry(pitch2)Rx(roll2) is for simplification, you don't really have to find the relation between yaw2 and pitch1,roll1 ,pitch_RS etc, since any Rotation matrix can be decomposed this way. I'm afraid you need to familiarize yourself with the basics of Euler angles and Rotation matrix Lie algebra.

[hq-cheng]

@QMonteCarlo wow...you answer was excellent!