GPSデータから運転曲線を描画するMATLABプログラム
https://scrapbox.io/files/6498f6a61db073001c496027.png
プログラムは以下の通り。Mappling Toolbox と Signal Processing Toolbox が必要。
code:MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Drawing speed trajectories from GPS data (.gpx)
% GPSデータから運転曲線を描画させるプログラム
% This is the main program.
%
% 2022/01/10 M. Miyatake: Newly developed
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%
% Initialization
%%%%%%%%%%%%%%%%%%%%%
clear; % Clear the workspace
%%%%%%%%%%%%%%%%%%%%%
% Definition of constants
%%%%%%%%%%%%%%%%%%%%%
a = 6378137.0;
b = 6356752.314245;
e = sqrt((a^2-b^2)/a^2);
%%%%%%%%%%%%%%%%%%%%%
% Reading gpx file
% The file name must be specified.
%%%%%%%%%%%%%%%%%%%%%
GPXdata = readgeotable(fullfile('GPSdata.gpx'));
%%%%%%%%%%%%%%%%%%%%%
% Extracting data from GPXdata
%%%%%%%%%%%%%%%%%%%%%
Latitude = GPXdata.Shape.Latitude;
Longitude = GPXdata.Shape.Longitude;
Height = GPXdata.Elevation;
Time = GPXdata.Time;
%%%%%%%%%%%%%%%%%%%%%
% Calculating elapsed seconds from 1st step
%%%%%%%%%%%%%%%%%%%%%
t0 = Time(1);
TimeD = [];
for (i=1:length(Time))
end
Second = seconds(TimeD);
%%%%%%%%%%%%%%%%%%%%%
% Calculating distance between two steps
%%%%%%%%%%%%%%%%%%%%%
dy = (Latitude(2:length(Latitude)) - Latitude(1:length(Latitude)-1))...
* 2 * pi / 360;
dx = (Longitude(2:length(Longitude)) - Longitude(1:length(Longitude)-1))...
* 2 * pi / 360;
mu_y = (Latitude(2:length(Latitude)) + Latitude(1:length(Latitude)-1))...
/ 2 * 2 * pi / 360;
W = sqrt( 1-(e^2) .* (sin(mu_y)) .^2 );
M = a .* (1-e^2) ./ (W.^3);
N = a ./ W;
dif = sqrt( (dy.*M).^2 + (dx.*N.*cos(mu_y)).^2 );
%%%%%%%%%%%%%%%%%%%%%
% Calculating speed
%%%%%%%%%%%%%%%%%%%%%
V_mps = dif ./ (Second(2:length(Second)) - Second(1:length(Second)-1));
V_kph = V_mps .* 3.6;
%%%%%%%%%%%%%%%%%%%%%
% Initial step adjustment
%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%
% Applying filter
%%%%%%%%%%%%%%%%%%%%%
% Zero-phase digital filter
d = designfilt('lowpassfir', ...
'PassbandFrequency',0.3,'StopbandFrequency',0.5, ...
'PassbandRipple',0.002,'StopbandAttenuation',40, ...
'DesignMethod','equiripple');
V_kph = filtfilt(d,V_kph);
% Moving average
%V_kph = movmean(V_kph,5);
%%%%%%%%%%%%%%%%%%%%%
% Calculating distance from 1st step
%%%%%%%%%%%%%%%%%%%%%
Dist = zeros(length(dif),1);
for (i=1:length(dif))
end
%%%%%%%%%%%%%%%%%%%%%
% Data plotting
%%%%%%%%%%%%%%%%%%%%%
tiledlayout('flow','TileSpacing','compact')
tiledlayout(2,2)
nexttile
plot(Dist,V_kph)
nexttile
plot(Second,V_kph)
nexttile
plot(Dist,Height)
nexttile
plot(Second,Dist)