For 循环无法按预期工作

Question

我正在计算 GPS 坐标（纬度、经度、高度）之间的距离和速度，当我使用单个值时，它似乎运行良好。但是，我有一个包含 79 个 GPS 坐标（3x79 矩阵）的矩阵，我想找到每两个连续点之间的距离和速度。当我尝试使用 for 循环时，除了第一个和最后一个值（非常高）之外，我得到的输出全为零。

我可能在做一些愚蠢的事情，但我可以发现它......任何建议都表示赞赏:)

for k=1:77
    R=6378.1e3; 

    latDistance = copter_llh(1,k+1) - copter_llh(1,k);
    lonDistance = copter_llh(2,k+1) - copter_llh(2,k);

    a = sin(latDistance / 2) * sin(latDistance / 2) + cos(copter_llh(1,k))...
      *cos(copter_llh(1,k+1)) * sin(lonDistance / 2) * sin(lonDistance / 2);
    c = 2 *atan2(sqrt(a), sqrt(1 - a));

    distance = R * c * 1000; % convert to meters

    height = copter_llh(3,k+1) - copter_llh(3,k); 
    distance = sqrt((distance^ 2) + (height^2)); 
    velocity = distance/0.1*60; 
    % stepsize =0.1min ___speed in m/s 
    distance(:,k)=(distance); 
    velocity(:,k)=(velocity); 
end %-----

Answer 1

您似乎无意中重用了 distance 变量，以及 for 循环末尾的 velocity 变量。您正在改变 distance 变量，然后尝试将其重塑为矩阵。您需要更改此变量，然后调用要存储矩阵的位置作为其他内容。此外，distance 和velocity 看起来像是单个数组，因此对第一个维度的: 访问是多余的。因此，请尝试这样做：

distance = zeros(1,77); %// NEW - Preallocate distance array
velocity = zeros(1,77); %// NEW - Preallocate velocity array
R=6378.1e3; %// NEW - Leave outside for loop.  Constant at each iteration
for k=1:77

    latDistance = copter_llh(1,k+1) - copter_llh(1,k);
    lonDistance = copter_llh(2,k+1) - copter_llh(2,k);

    a = sin(latDistance / 2) * sin(latDistance / 2) + cos(copter_llh(1,k))...
      *cos(copter_llh(1,k+1)) * sin(lonDistance / 2) * sin(lonDistance / 2);
    c = 2 *atan2(sqrt(a), sqrt(1 - a));

    dist = R * c * 1000; %// convert to meters - NEW - change variable

    height = copter_llh(3,k+1) - copter_llh(3,k); 
    %// stepsize =0.1min ___speed in m/s 
    distance(k) = sqrt((dist^ 2) + (height^2)); %// NEW - Assign directly to distance array
    velocity(k) = distance/0.1*60; %// NEW - Assign directly to velocity array
end %-----

此代码现在应存储 77 个距离和速度，分别存储在 distance 和 velocity 中。试试看，让我知道它是否有效！

---

旁注

您可以以完全矢量化的方式计算它，而无需任何for 循环。您可以使用diff 为您计算这些相邻距离。因此，您实际上可以执行以下操作：

R=6378.1e3;
latDistance = diff(copter_llh(1,:)); % // NEW
lonDistance = diff(copter_llh(2,:)); %// NEW

a = sin(latDistance / 2) .* sin(latDistance / 2) + cos(copter_llh(1,1:end-1))...
  .*cos(copter_llh(1,2:end)) .* sin(lonDistance / 2) .* sin(lonDistance / 2);
c = 2 *atan2(sqrt(a), sqrt(1 - a));

dist = R * c * 1000; %// convert to meters - NEW - change variable

height = diff(copter_llh(3,:));
distance = sqrt((dist.^2) + (height.^2));  %// NEW
velocity = distance/0.1*60; %// NEW

上面的代码应该等同于您的for 循环方法，但请记住始终先编写对您有意义的代码，然后再开始进行任何优化。这是最好的编码方式！