%% parameters eps = 0.000001; s = 0.0 ; c =0.0; cp = 0.0; Q = 0.0; m = 0.0; function rpy = TF(R) %% output rpy = zeros(1,3); % rpy(1) roll % rpy(2) pitch % rpy(3) yaw %% function description % 2:XYZ固定角变换; rpy(2) = atan2(-R(3,1),sqrt(R(1,1)*R(1,1)+R(2,1)*R(2,1))); if(abs(abs(rpy(2))-pi/2)<eps) if(rpy(2)>0) rpy(2) = pi/2 ; rpy(3) = 0.0 ; rpy(1) = atan2(R(1,2),R(2,2)) ; else rpy(2) = -pi/2 ; rpy(3) = 0.0 ; rpy(1) = -atan2(R(1,2),R(2,2)) ; end else cp = cos(rpy(2)) ; rpy(3) = atan2(R(2,1)/cp,R(1,1)/cp) ; rpy(1) = atan2(R(3,2)/cp ,R(3,3)/cp) ; end end
时间: 2024-01-02 16:03:08 浏览: 98
C 代码 在从 A 到 B 的整数中搜索值 J,使得 F(J) = C.rar
这是一个用于计算旋转矩阵 R 对应的欧拉角 rpy 的函数。函数中定义了一些参数和变量,然后根据旋转矩阵的值计算出对应的欧拉角。
在函数中,首先初始化一个大小为 1x3 的零向量 rpy,用来存储计算得到的欧拉角。然后根据 XYZ 固定角变换的公式,计算出 rpy(2),即 pitch 角。如果 pitch 角的绝对值与 pi/2 的差小于一个极小的值 eps,则判断 pitch 角是否大于 0,若大于 0 则设置 rpy(2) 为 pi/2,rpy(3) 为 0,rpy(1) 为 atan2(R(1,2),R(2,2));若小于 0 则设置 rpy(2) 为 -pi/2,rpy(3) 为 0,rpy(1) 为 -atan2(R(1,2),R(2,2))。否则,计算 cp = cos(rpy(2)),然后根据公式计算 rpy(3) 和 rpy(1)。
最后,返回计算得到的欧拉角 rpy。
阅读全文