Question

我正在尝试更快地计算一段代码，并希望摆脱for循环。我有三个向量滚动，俯仰和航向。我必须得到每个滚动，俯仰和航向值的旋转矩阵。现在，我的代码是

for i=1:size(roll,1)
    R1_r = [1 0 0; 0 cos(roll(i)) -sin(roll(i)); 0 sin(roll(i)) cos(roll(i))];
    R2_p = [cos(pitch(i)) 0 sin(pitch(i)); 0 1 0; -sin(pitch(i)) 0 cos(pitch(i))];
    R3_h = [cos(head(i)) sin(head(i)) 0; -sin(head(i)) cos(head(i)) 0; 0 0 1];
    R = R3_h*R2_p*R1_r;
    x1(i,2:4) = (R*x(i,2:4)')';
end

我想替换整个for循环。有没有办法做到这一点？

Answer 1

以下代码将为您展开上一个产品：

syms roll pitch yaw x1 x2 x3

R1 = [1 0 0; 0 cos(roll) -sin(roll); 0 sin(roll) cos(roll)];
R2 = [cos(pitch) 0 sin(pitch); 0 1 0; -sin(pitch) 0 cos(pitch)];
R3 = [cos(yaw) sin(yaw) 0; -sin(yaw) cos(yaw) 0; 0 0 1];
R  = R3*R2*R1;
x  = [x1;x2;x3];

R*x

输出：

 x2*(cos(roll)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll)) - x3*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)) + x1*cos(pitch)*cos(yaw)
 x2*(cos(roll)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw)) - x3*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)) - x1*cos(pitch)*sin(yaw)
                                                                          x3*cos(pitch)*cos(roll) - x1*sin(pitch) + x2*cos(pitch)*sin(roll)

基于此，你应该采用角度的正弦和余弦：

cp = cos(pitch); sp = sin(pitch);
cr = cos(roll);  sr = sin(roll);
cy = cos(yaw);   sy = sin(yaw);

并替换上述公式中的x1 -> x1(:,2)，x2 -> x1(:,3)和x3 -> x1(:,4)，并在需要时将*符号更改为.*。

最终代码应如下所示：

x1_new = x1(:,3).*(cr.*sy + cy.*sp.*sr) - x1(:,4).*(sr.*sy - cr.*cy.*sp) + x1(:,2).*cp.*cy;
x2_new = x1(:,3).*(cr.*cy - sp.*sr.*sy) - x1(:,4).*(cy.*sr + cr.*sp.*sy) - x1(:,2).*cp.*sy;
x3_new = x1(:,4).*cp.*cr - x1(:,2).*sp + x1(:,3).*cp.*sr;

小心不要随意更换x1的列;您需要将结果存储在新列中，否则您将破坏计算的其余部分。在这三行之后，您可以安全地替换x1列。

没有for循环的矩阵计算

1 个答案: