Best way to import GPS track for point mass simulation?

[tim_pattinson]

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 (https://ir.library.oregonstate.edu/concern/graduate_thesis_or_dissertations/tx31qm51z) (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.
1270
Cheers,
Tim

[DougMilliken]

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?

[rory.gover]

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).

[BillCobb]

see attached.1275

[DougMilliken]

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.

[BillCobb]

Just 2 of many options.12761277

[BillCobb]

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:121278
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')

[Steve Krug]

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
load('Lincoln_2015_Endurance_GPSCoordinates') %Input data from AIM/GPS

%% 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]
latRad = zeros(1,length(longDeg));
longRad = zeros(1,length(longDeg));
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)
latRad = latDeg(i)*(pi/180); %Convert from deg to radians
longRad = longDeg(i)*(pi/180); %Convert from deg to radians

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
%% Filter Corner Radii and change/offset low corner radii
%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(:,2) = flipud(rFilt); %corner radius
trackName(:,3) = 1; %endurance mode?
trackName(:,4) = flipud(distancetest); %cumulative distance
%%

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

j = 1;
for i = 1:length(trackName)-1
radiiUpsample(j:j+upsampleRate) = linspace(trackName(i,2)...
,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 = transpose(zeros(4,length(radiiUpsample)));
upsampled_finalTrack(:,1) = dxUpsample;
upsampled_finalTrack(:,2) = radiiUpsample;
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]')
ylabel('Radius [m]')
ylim([0 100])
title('Corner Radius vs. Distance, Lincoln')


hold off

%%

[Steve Krug]

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

load('C:\Users\stkrug\Documents\MATLAB\LagunaSeca. mat')

% 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
radius_raw = transpose(zeros(1,length(accel)));
%Calculate Radius
for i = 1:length(accel)

radius_raw(i) = (velocity(i)^2)/abs(accel(i));
if radius_raw(i) > 10000
radius_raw(i) = 3000;
end
end

radius = filtfilt(H,G,radius_raw);

figure(3)
plot(time, distance_raw, time, distance);
legend('raw','filtered')
%Calculate dx
dx = transpose(zeros(1,length(radius)));
for j = 1:length(radius)-1
dx(j) = distance(j+1) - distance(j);
end

figure(4)
plot(time, radius_raw, time, radius)

figure(5)
plot(distance, radius_raw, distance, radius)


%% Create Input File for LapSim

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

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

%%

[Steve Krug]

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:

1279

1280

1281

[BillCobb]

Am I missing something ? A race track has finite width lanes for which you shoehorn the car thru a racing line path. This 'line' will be different for each of vehicle and tire parameter settings.

To be specific, is the data being shown here == 'THE LINE' or is it just a walk around the center of the track map ?

BTW: with a little handle graphics you can study the track mapping problems a bit better. I'd use radius because it relates to my sense of surveying, but here is curvature to be PC.
I needed a crutch to figure out where I was on the track.

1282

[BillCobb]

Here is an excellent paper by Honda on the subject of Lap Time Simulation. (Speaking of which by the O.P.) Go 4 it. Log-in to download and study.

https://www.hondarandd.jp/point.php?pid=1264&lang=en

1283

[BillCobb]

Here is an excellent paper by Honda on the subject of Lap Time Simulation. (Speaking of which by the O.P.) Go 4 it. Log-in to download and study.

https://www.hondarandd.jp/point.php?pid=1264&lang=en

1283

[jd74914]

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).

Agreed. If you are going with a filtering method (rather than the above curve-fit to arc methods), working with the raw signal is often preferential. Not sure what GPS noise looks like, but it's likely possible to remove. I'd at least recommend trying to remove some before the differentiation and taking a second pass at the final data only if necessary. I work in the image processing world (laser diagnostics) and most of our filtering is to reduce dark noise, Gaussian noise, etc. which can often be done relatively easily before doing heavy math on the data.

[Z]

IT'S AN OUTRAGE!
==============

Just 2 of many options.http://www.fsae.com/forums/attachment.php?attachmentid=1276&stc=1http://www.fsae.com/forums/attachment.php?attachmentid=1277&stc=1

Bill,

I am very disappointed in your Northern Hemispheric imperialist attitude shown above!

And shame on all you 'Strayan students for being so slow to protest this outrage!
~o0o~

What is the old-fart going on about?

Well, Bill has drawn the Calder Park track-map from a viewpoint somewhere around the North Pole. Or more likely from his secret underground bunker, somewhere under North Merca. Namely, from deep underground, looking UP at the track.

Geez, what will this do to the laptimes???

I suspect the first few cars will be very slow, given the very long tunnel they will have to dig under the bitumen. Then there is the inverted gravitational field to contend with. And perhaps even negative laptimes ... but will that be good or bad for points? On the upside, driving underground might mean no problems hitting the cones, err...???

Aarghhh, all this standing on my head trying to figure this out is hurting my brain!
~o0o~

Tim,

Bill got it right in a later post where he questioned why you want to follow "the line" that was set by a vehicle probably a lot longer and wider than the average FSAE car. And travelling much slower.

Methinks that all you really need is the positions of the cones. The track-map (right-way-up, of course!), together with some of the abundant video footage available, should allow you to reasonably accurately determine those cone positions.

Then on to the whole business of writing a HALF-DECENT VD-simulator. "Point-mass" is oh-so last century...

Z

[BillCobb]

Z: It was the only way I could get you to bite on the total concept at hand from an unblemished, unMatlabed and unOptimumed podium. No need for gpxread, just a couple of moving median filters. Who cares what the track is. Can they address the real problem(s) using Excel ? Or in your case Visual Basic.

I have a few other cone deliniated tracks that have been a challenge. Just what do the children say about this one ?1284

[Z]

Just what do the children say about this one ?http://www.fsae.com/forums/attachment.php?attachmentid=1284&stc=1

Hmmm? Well, that latest track-map looks almost exactly like the previous two, just with the filtering cranked up a few more notches. In fact, just a smidge more smoothing and laptime simulation will be a doddle. Pure Ay all the way around. This FSAE laptime optimisation is getting easier each day!

(Tim, I hope you are paying attention...)

Z

[BillCobb]

Just to clarify: THIS is a racetrack. Notice the steel and concrete retainers ? The best path thru this maize is the science project, not like a single track1285 copied from a golf cart going 6 - 7 kph down the road.

[DougMilliken]

It's possible to draw a reasonable racing line automatically. See RCVD p.341 (bottom of the page). The "best" line varies with the vehicle capability...

Going back to Bill's oval, "Just what do the children say about this one ?"
On my screen (aspect ratio) it looks like a circle will fit within the edges of the track, leaving just enough for the half-track-width of the car.

[Michael Royce]

Bill,
Thought that track looked familiar!! It's the current Silverstone F1 track

Michael.