MATLABでGPSデータから距離を計算

GPSの緯度経度の行列から移動距離を計算するアルゴリズムがmatlab公式に用意されてないので自分で書きました。 探せばあるのかもしれません。

今回の手法ではヒュベニの公式というのを使います。 詳細な説明等はここのサイトがよいです。

画像も借りてきました。

関数は以下の通り。整備したらGithubにでも投げます。

function D = calcGPS_dist(LON,LAT)

len = length(LON);

D = 0;
for i = 1:len-1
    D = D+hubeny([LON(i),LAT(i)],[LON(i+1),LAT(i+1)]);
end

end

function d = hubeny(L1,L2)
% input: pair of [LON,LAT]
% output : distance [m]
% e2 = 0.00669437999019758;
a = 6378137;
b = 6356752.314140;
e2 =   (a^2 - b^2) / a^2;

mu_y = (L1(2) + L2(2))/2;
W = sqrt(1-e2*(sind(mu_y))^2);
M = a*(1-e2)/(W^3);
N = a/W;

dx = (L1(1)-L2(1))/180*pi;
dy = (L1(2)-L2(2))/180*pi;

d = sqrt( (dy*M)^2+(dx*N*cosd(mu_y))^2 );

end

ハマリポイントは角度をradに変換するのを忘れていたところです。