  # Thread: Best way to import GPS track for point mass simulation?

1. ## Best way to import GPS track for point mass simulation?

Hi all,
Tim here from RMIT Electric Racing.

I am working on a simple point mass simulator in MATLAB using the 'critical points' method as described in Chris Patton's thesis (no yaw dynamics yet though)

In his work, he uses a simple test track with two corners, a straight and a slalom. GPS data is used and the track curvature is calculated using the first and second x and y derivatives, and filtfilt is used in MATLAB to smooth the resultant curvature.

I have successfully replicated this using the GPS file released by SAE-A from the 2016 endurance/autox track, however the calculated lap time is quite sensitive to the method used to derive the curvature from the GPS log.

For example with parameters from the RMIT-E 2016 car. (lots of power, no downforce)
Smoothing with csaps(), p=0.1, laptime= 86.38s
csaps, p=0.9, laptime = 87.78
pchip, laptime = 88.24s
Butterworth filter filtfilt 0.25hz 2nd order (as in Chris Patton's thesis) , laptime = 80.65s (!!!!)

For reference, the best time at the 2016 event was ~76s, we got ~79.5s as our best time.

The improvement in lap time with the Butterworth filter is coming from it rounding off the peaks of the curvature substantially, however I suspect that those peaks are an artifact of the curvature calculation to start off with.
Here is a plot with the different methods of curvature filtering, bottom scale is distance in m

https://i.imgur.com/j99BjLy.png

I would appreciate any suggestions on how to best treat this data, or if there is another method I could use.

I have attached the GPS file and the matlab script I am currently using if anyone would like to have a look.
lapsim_.zip
Cheers,
Tim  Reply With Quote

2. Originally Posted by tim_pattinson Smoothing with csaps(), p=0.1, laptime= 86.38s
csaps, p=0.9, laptime = 87.78
pchip, laptime = 88.24s
Butterworth filter filtfilt 0.25hz 2nd order (as in Chris Patton's thesis) , laptime = 80.65s (!!!!)
For reference, the best time at the 2016 event was ~76s, we got ~79.5s as our best time.
You may have (inadvertently?) just shown why the very top drivers are well paid(grin).

My questions:
What are you trying to learn or accomplish with this modeling effort?
Do all these different filters keep the car between the cones on the original track?  Reply With Quote

3. Nb. the output of a GPS signal is lat & long plus noise, which can be converted to relative x & y coords plus noise without adding in additional noise. In order to calculate curvature, you need the single and double derivatives of x & y wrt arc length. Differentiating noisy signals is generally Bad News Bears.

In general, it's best to filter raw signals and then apply math to it as necessary, rather than applying math and then filtering (advice given with all relative caveats and exceptions, particularly related to statistical smooshing - i.e. Kalman filters).  Reply With Quote

4. ## GPS filter

see attached.Aus2016.jpg  Reply With Quote

5. Originally Posted by BillCobb see attached.
Ha!! I don't have the Signal Processing Toolbox so can't test directly, but I'm sure that heavily filtering really straightens out the corners.

Back when we were running our LTS, we used an in-house zero-phase-loss filter (very simple) to smooth out manually entered racing lines. This was early 1980s, there was no GPS or dead-reckoning data available to us. Instead, we worked from paper maps of the tracks. I think I still have the blueprints for the first Detroit GP (city street course) that we obtained from the city's civil engineer, several months before the race.  Reply With Quote

6. ## Cones Fear Me.

Just 2 of many options.Lap It Up_1.jpgLap It Up_2.jpg  Reply With Quote

7. ## Tracking Curvaciousness

But wait, curvature is just the inverse of the track radius at each section. It takes 3 points to define a circle which they would lie on, and so
all you need to do is traipse thru the track data and figure the radii of all those circles. When you are done, flip them over and you have the
track curvatures. Like so: ( I took some latitude with the units to set the track size to metery-like dimentia). The radii come out looking
pretty sane, if I may say so. No heavy lifting toolboxes required !

fid=fopen('Aus2016.gpx');

for n=1:12Aus2016_curvature.jpg
str = fgetl(fid); %Preamble crap
end

for n=1:200
str=fgetl(fid);
if length(str) < 25
break
else
lat(n)=str2double(str(13:22));
lon(n)=str2double(str(30:39));
end
end

lat = (lat - lat(1))*100000; % The center of OZ.
lon = (lon - lon(1))*100000;

figure
plot(lat,lon,'bo'),hold on,axis equal

figure,hold on,grid on
for n=3:length(lat)-4
x=lat(n:n+2);
y=lon(n:n+2);
mx = mean(x);
my = mean(y);
X = x - mx;
Y = y - my; % Get differences from means
dx2 = mean(X.^2);
dy2 = mean(Y.^2); % Get variances
t = [X,Y]\(X.^2-dx2+Y.^2-dy2)/2; % Solve least mean squares problem
a0 = t(1);
b0 = t(2); % t is the 2 x 1 solution array [a0;b0]
r(n) = sqrt(dx2+dy2+a0^2+b0^2); % Calculate the radius
a = a0 + mx;
b = b0 + my; % Locate the circle's center
curv(n) = 1/r(n); % Get the curvature
end
plot(curv,'k.-')
ylim([0 .5])
grid on
xlabel('Starting marker (Cone?)')
ylabel('Curvature (1/m)')
title('Aus215.gpx Track Data')  Reply With Quote

8. I have a very similar code, Bill Cobb!

The code is pretty gross, but it will take latitude and longitude points and convert into discrete corner radii. Basically it constructs an arc between every subsequent 3 points.

%% Latitude, Longitude to Cartesian to Corner Radius and input file type for LapSim
% Author: Steve Krug
% Last Updated: 2/17/16
clear all

upsampleRate = 3;
sampleFactor = 0.25;
%% Load data, variables names must be latDeg, longDeg, and distance

%% Segment data
latDeg = latDeg(100:5125);
longDeg = longDeg(100:5125);
distance = distance(100:5125);

%% Input Data and Initialize
r = 6371.0088*1000; %Radius of earth in [m]
x = zeros(1,length(longDeg));
y = zeros(1,length(longDeg));
xc = zeros(1,length(longDeg));
yc = zeros(1,length(longDeg));
%% Filter GPS Coordinates for smoother radius calculations
d1 = designfilt('lowpassiir','FilterOrder',10, ...
'HalfPowerFrequency',.025,'DesignMethod','butter') ;
latDeg = filtfilt(d1,latDeg);
longDeg = filtfilt(d1,longDeg);
%%

%% Main Loop to convert to Cartesian Coordinates
for i = 1:length(latDeg)

x(i) = r*cos(latRad)*cos(longRad); %Distance is in [m] relative to center of earth
y(i) = r*cos(latRad)*sin(longRad); %Distance is in [m] relative to center of earth
end
%%

% figure
% hold all
% scatter(latDeg,longDeg, 'xb')
%
% % Filter GPS Coordinates for smoother radius calculations
% d1 = designfilt('lowpassiir','FilterOrder',15, ...
% 'HalfPowerFrequency',0.1,'DesignMethod','butter');
% latDeg = filtfilt(d1,latDeg);
% longDeg = filtfilt(d1,longDeg);
% %
% scatter(latDeg,longDeg, '.r')
% xlabel('GPS Latitude [deg]')
% ylabel('GPS Longitude [deg]')
% title('Vehicle Position Filtered vs. Unfiltered GPS Coordinates')
% hold off

%Change to offset
% lowerOffset = 0;
% upperOffset = 0;
% x = x(1+lowerOffset length(x)-upperOffset));
% y = y(1+lowerOffset length(y)-upperOffset));

distance = distance*1; %Convert to m

%% Offset local coordinate system
%offsets with different tracks, not necessary but translates axis values
%bceause more intuitive
for j = 1:length(x)
xc(j) = x(j)-min(x);
yc(j) = y(j)-min(y);
end
%%

figure
plot(xc,yc)

%% Calculate corner Radii, filter, and scale
%Note: Calculation takes off 3 data to remain in bounds of array
for k = 1:length(x)-3
r(k) = sqrt( ...
((xc(k+1) - xc(k))^2 + (yc(k+1) - yc(k))^2)...
*((xc(k+1) - xc(k+2))^2 + (yc(k+1) - yc(k+2))^2)...
*((xc(k+2) - xc(k))^2 + (yc(k+2) - yc(k))^2) )/...
( 2*abs( xc(k)*yc(k+1) + xc(k+1)*yc(k+2) + xc(k+2)*yc(k)...
- xc(k)*yc(k+2) - xc(k+1)*yc(k) - xc(k+2)*yc(k+1) ) );
end
%rFilt = filtfilt(d1, r); % Apply filter

%Offset
r = r + 7;

rFilt = r; % No filter
for q = 1:length(rFilt)
if rFilt(q) > 10000
r(q) = 5000;
rFilt(q) = 5000;
end
% if rFilt(q) < 4
% rFilt(q) = rFilt(q)+5;
% r(q) = r(q)+5;
% end
end

r

%%
% for n = 1:length(r)-20
% if r(n) == inf
% for h = 1:10
% fixInf(h) = r(h+n);
% end
% %fixInf(h) = [r(n):r(n+10)];
% r(n) = min(fixInf);
% end
% end

%% Make distance same array size as radii
for u = 1:length(distance)-3
distancetest(u) = distance(u);
end
%%

%% Calculate change in distance per increment step
dx = zeros(1,length(r));
for t = 1:length(distance)-3
dx(t) = distance(t+1)-distance(t);
end
%%

%% Create track input file for LapSim
dx = transpose(dx);
rFilt = transpose(rFilt);
%r = flipud(r);
distancetest = transpose(distancetest);

trackName = transpose(zeros(4,length(rFilt)));
trackName(:,1) = flipud(dx); %distance increment
trackName(:,3) = 1; %endurance mode?
trackName(:,4) = flipud(distancetest); %cumulative distance
%%

%% Interpolate Between Data
%Initialize
distanceUpsample = transpose(zeros(1,(length(trackName)*upsampleRate) ));
dxUpsample = transpose(zeros(1,length(trackName)*upsampleRate)) ;

j = 1;
for i = 1:length(trackName)-1
,trackName(i+1,2), upsampleRate+1);
distanceUpsample(j:j+upsampleRate) = linspace(trackName(i,4)...
,trackName(i+1,4), upsampleRate+1);

j =j+upsampleRate;
end

for u = 1:length(dxUpsample)-1
dxUpsample(u) = distanceUpsample(u) - distanceUpsample(u+1);
end

upsampled_finalTrack(:,1) = dxUpsample;
upsampled_finalTrack(:,3) = 1;
upsampled_finalTrack(:,4) = distanceUpsample;

upsampled_finalTrack = upsampled_finalTrack((1:length(upsampled_finalTrac k)-upsampleRate), ;

%% Final Track
finalTrack = trackName;

figure
hold all
%plot(flipud(finalTrack(:,4)),r)
scatter(upsampled_finalTrack(:,4),upsampled_finalT rack(:,2))
xlabel('Distance [m]')
ylim([0 100])

hold off

%%  Reply With Quote

9. Or you can just use a = v^2/r...

Again, excuse the quick 'n dirty code.

%Accelerometer and Velocity Data Track
%Columns: distance [m], speed [m/s], acceleration [m/s]
clear all
close all
clc

% distance = distance;

%% Filter Accel Data
% filt1 = designfilt('lowpassiir','FilterOrder',6, ...
% 'HalfPowerFrequency',.055,'DesignMethod','butter') ;
% filt2 = designfilt('lowpassiir','FilterOrder',1, ...
% 'HalfPowerFrequency',.075,'DesignMethod','butter') ;

%% Filters
ts = 0.01;
newFreq = 100;
d1 = designfilt('lowpassiir','FilterOrder',3, ...
'HalfPowerFrequency',0.020,'DesignMethod','butter' );
d2 = designfilt('lowpassiir','FilterOrder',3, ...
'HalfPowerFrequency',0.25,'DesignMethod','butter') ;
d3 = designfilt('lowpassiir','FilterOrder',10, ...
'HalfPowerFrequency',0.1,'DesignMethod','butter');
% Filters for profile roughness rebuilding, zero mean.
% (cutoff freq/(sample freq/2))
filt_freq = [0.05/(newFreq/2);... % Accelerations high pass frequency
0.05/(newFreq/2);... % Velocity high pass frequency
0.05/(newFreq/2);... % Displacement high pass frequency
2/(newFreq/2);... %general low pass settings
0.1/(newFreq/2);... %Isolate MT filter, Acc
0.1/(newFreq/2);... %Isolate MT filter, Vel
0.001/(newFreq/2);]; %isolate MT filter, Displ

[B,A]=butter(3,filt_freq(1),'High');
[D,C]=butter(3,filt_freq(2),'High');
[F,E]=butter(3,filt_freq(3),'High');
[H,G]=butter(1,filt_freq(4),'Low'); %general low pass settings
[J,I]=butter(1,filt_freq(5),'Low'); %Isolate MT filter, Acc
[L,K]=butter(1,filt_freq(6),'Low'); %Isolate MT filter, Vel
[N,M]=butter(1,filt_freq(7),'Low'); %Isolate MT filter, Displ

%%
time = linspace(0, 105.85, length(Vx));
velocity = filtfilt(H,G,Vx);

figure(1)
plot(time ,Vx, time, velocity)
legend('raw','filtered')

accel = Ay;
accel = filtfilt(H,G,accel);

distance_raw = cumtrapz(velocity)*ts;
% distance_MT = filtfilt(N,M,distance_raw); %Isolate MT
% distance = filtfilt(F,E,distance_raw-distance_MT); %high pass filter, and subtract MT
distance = filtfilt(H,G,distance_raw);

hold on

% plot(distance, velocity)
% plot(distance)
% plot(distance_raw)
figure(2)
plot(time, Ay, time, accel);
legend('raw','filtered')

hold off

%Intialize
for i = 1:length(accel)

end
end

figure(3)
plot(time, distance_raw, time, distance);
legend('raw','filtered')
%Calculate dx
dx(j) = distance(j+1) - distance(j);
end

figure(4)

figure(5)

%% Create Input File for LapSim

LagunaSeca = transpose(zeros(4,length(100:10500)));

LagunaSeca(:,1) = dx(100:10500); %Distance increment
LagunaSeca(:,3) = 1; %Endurance mode?
LagunaSeca(:,4) = distance(100:10500); % Cumulative Distance

%%  Reply With Quote

10. It might be possible to brute-force or optimize (using an fmincon or fminsearch-like function in MatLab) your filter settings to achieve the best correlation to test data. Would require coupling lap time simulation results to the track maker. The objective function for the optimization would be minimizing error between simulation and test data, or some other error function that establishes it's value based on a more specific objective function.

The following was guess and check:

fitting_attempts.JPG

accel_aero.JPG

LTS_correlation.jpg  Reply With Quote

Page 1 of 2 1 2 Last 