Getting the total turning angle of a vehicle's route

I have this spreadsheet (which is attached) which contains a vehicle's speed and location as it went on a path.

I have mapped the location data in local coordinates: the code to generate the graph is below:

How do I get the total turning angle? I want it so that the total turning angle increases whenever the heading changes (so an absolute change) with respect to time. I have an idea which involves finding the direction between two points and calculating the change in direction wrt the next point but I have no idea how to implement this in MATLAB.

A = readmatrix("20181024173751_947BE7249B00.csv");

figure(2)
%column 3 is longitude, column 4 is latitude
[xEast, yNorth] = latlon2local(A(:, 4), A(:, 3), 0, [A(1, 4), A(1, 3), 0]);
plot(xEast, yNorth)
xlim([-9e4, 9e4])
ylim([-9e4, 9e4])
xlabel('X (m)')
ylabel('Y (m)')
title('Route 24/10/2018 17:37:51')
grid on;

[Amritz Ansara]

Doesn't have to necessarily be with respect to time, just with respect to how many points have been considered. But the time data is available too on the first column in UNIX millisecond form.

[Adam Danz]

What do you mean by total turning angle?

> I want it so that the total turning angle increases whenever the heading changes

Heading is a velocity vector consisting of a direction (orientation) and speed. I think what you're asking for is the orientation at each point (xEast, yNorth)?

Note that the instantaneous orietnation will be wildly noisy if computed by the raw data. For example, look at points [26 27 28] from your raw data. The orientation changes considerable between these 3 points within a matter of milliseconds. Look at points 30 to 35 and you'll see another example of drastic changes within ms.

You'll likely need to smooth your positional data first. Were you planning on doing that or do you want the very noisy instantaneous orientation data computed from the raw positional data?

I work with steering data on a regular basis (see example but I collect translational and angular velocity data and compute position which is a lot less noisy than doing the reverse).

[Amritz Ansara]

I want the absolute difference between the orientation of one point, and the orientation of the next point, and then sum across all the points. The difference between the orientation of one point and the orientation of the other point is due to the rotation of the vehicle, however small it is. I want the total absolute rotation the vehicle has experienced on the journey.

For example, if point a is oriented at 95 degrees and point b after it is oriented at 97 degrees, the turning angle as I have mentioned above is 2 degrees. If point c is now oriented at 94 degrees, the absolute turning angle between b and c is 3 degrees. The total turning angle so far, that I want, is therefore 5 degrees.

I see now that the noise might affect things, but let's assume that I want what I've said just now even if the data is noisy.

Answer 1

[Adam Danz]

This should get you started if I've understood you correctly.

I've calculated orientation in two different way. First, observer-centered orientation where orientation(j) is the angle between positions j-1 and j. Second, world-centered orientation where theta(j) is the orientation of position j relative to the first position coordinate. Since your position coordinates start at (0,0) these values are somewhat similar but the obverser-centered orientation is a lot noisier due to differentiating noisy positional signals. The observer-centered orientation also needs additional clean-up since circular orientation values wrap to -180,180 or 0,360 so when orientation crosses those thresholds, the orientation signal will jump to the other end of the axis in cartesian coordinates (not polar coordinates). You can easily see this in the plots below where there are near vertical lines that jump from the top/bottom of the axes.

The plot below shows the raw data in cartesian coordinates, the raw data in polar coordinates, and the first 100 position values to see details. It also shows both computations of orientation (see legend).

The timestamps are not needed since orientation is scalar (unlike angular velocity which would require time intervals).

Lastly, this answer does not take the last step you need to compute the absolute difference in orientation. If I understand you correctly, you can compute that by taking the sum of the absolute differentiated orientation values: sum(abs(diff(angle)).

This uses two functions from the mapping toolbox. If you do not have that toolbox the two functions will be easy to replace.

A = readmatrix("https://www.mathworks.com/matlabcentral/answers/uploaded_files/621563/20181024173751_947BE7249B00.csv");
figure()
tiledlayout(3,3);   % added

% Show all position coordinates in cartesian coordinates
nexttile            % added
hold on             % added
[xEast, yNorth] = latlon2local(A(:, 4), A(:, 3), 0, [A(1, 4), A(1, 3), 0]);
p1 = plot(xEast(1), yNorth(1), 'rx','MarkerSize',9,'LineWidth',2,'DisplayName','First coordinate'); % added
p0 = plot(xEast, yNorth, 'b.','DisplayName','Raw data');        % added marker
axis equal % added
xlim(max(abs(xEast(:))).*[-1,1])  % changed
ylim(max(abs(yNorth(:))).*[-1,1]) % changed
xlabel('X (m)')
ylabel('Y (m)')
title('Route 24/10/2018 17:37:51')
grid on;

% Show all position coordinates in polar coordinates
[theta,rho] = cart2pol(xEast-xEast(1), yNorth-yNorth(1));  % must start at (0,0)
theta = wrapToPi(theta); % mapping toolbox
nexttile    
polarplot(theta(1), rho(1), 'rx', 'MarkerSize', 9, 'LineWidth',2)
hold on
polarplot(theta, rho, 'b.')  
title('Position data in polar coordinates')

% Show first 100 coordinates 
nexttile
hold on
plot(xEast(1), yNorth(1), 'rx', 'MarkerSize', 9, 'LineWidth',2) 
plot(xEast(1:100), yNorth(1:100), 'b.') 
axis equal 
xlabel('X (m)')
ylabel('Y (m)')
title('First 100 positions')
grid on;

% Compute orientation
orientation = [0;atan2d(diff(yNorth),diff(xEast))];  % nan are from no motion, I think
orientation = wrapTo180(orientation);  %mapping toolbox

nexttile([1,3])
hold on
p2 = plot(1:numel(yNorth),orientation, 'o-','MarkerSize',3,'MarkerFaceColor','w','DisplayName','Observer-centered');
p3 = plot(1:numel(yNorth),theta*180/pi, 'm-', 'LineWidth', 1, 'DisplayName','World-centered');
grid on
yline([-90 0 90])
xlabel('Coordinate index')
ylabel('orientation (deg)')
title('Orientation from raw position data')
ylim([-180 180])

nexttile([1,3])
hold on
plot(1:100, orientation(1:100), 'o-','MarkerSize',3,'MarkerFaceColor','w')
plot(1:100,theta(1:100)*180/pi, 'm-', 'LineWidth', 1)
grid on
yline([-90 0 90])
xlabel('Coordinate index')
ylabel('orientation (deg)')
title('first 100 orientations')
xlim([1,100])
ylim([-180 180])

lg = legend([p0,p1,p2,p3],'Orientation','Horizontal'); 
lg.Layout.Tile = 'South';

[Amritz Ansara]

Lovely, this is what I want. Thank you :) I shall perform the cleanup myself!

[Adam Danz]

Sounds good. Glad I could help.