+ Reply to Thread
Page 1 of 2 1 2 LastLast
Results 1 to 10 of 20

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

  1. #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

  2. #2
    Senior Member
    Join Date
    Mar 2008
    Location
    Buffalo, NY USA
    Posts
    340
    Quote Originally Posted by tim_pattinson View Post
    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?

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

  4. #4
    Senior Member
    Join Date
    Mar 2008
    Location
    Brighton, MI
    Posts
    686

    GPS filter

    see attached.Aus2016.jpg

  5. #5
    Senior Member
    Join Date
    Mar 2008
    Location
    Buffalo, NY USA
    Posts
    340
    Quote Originally Posted by BillCobb View Post
    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.

  6. #6
    Senior Member
    Join Date
    Mar 2008
    Location
    Brighton, MI
    Posts
    686

    Cones Fear Me.

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

  7. #7
    Senior Member
    Join Date
    Mar 2008
    Location
    Brighton, MI
    Posts
    686

    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')
    Last edited by BillCobb; 01-07-2018 at 09:31 PM.

  8. #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
    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+lowerOffsetlength(x)-upperOffset));
    % y = y(1+lowerOffsetlength(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
    Wisconsin Racing

  9. #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

    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
    Wisconsin Racing

  10. #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
    Steve Krug
    Wisconsin Racing

+ Reply to Thread
Page 1 of 2 1 2 LastLast

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts