How to calculate the point-by-point radius of curvature of a trajectory that is not a proper function - matlab

Question

with Matlab i'm trying to calculate the "radius of curvature" signal of a trajectory obtained using GPS data projected to the local cartesian plane.
The value of the signal onthe n-th point is the one of the osculating circle tangent to the trajectory on that point.
By convention the signal amplitude has to be negative when related to a left turn and viceversa.
With trajectories having a proper function as graph i'm building the "sign" signal evaluating the numeric difference between the y coordinate of the center of the osculating circle:
for i=1:length(yCenter) -1
aux=Y_m(closestIndex_head:closestIndex_tail );
if yCenter(i) - aux(i) > 0
sign(i)=-1;
else
sign(i)=+1;
end
end
yCenter contains x-coordinates of all osculating circles related to each point of the trajectory;
Y_m contain the y-coordinates of every point in trajectory.
The above simple method works as long as the trajectory's graph is a proper function (for every x there is only one y).
The trajectory i'm working on is like that:
and the sign signal got some anomalies:
The sign seems to change within a turn.
I've tried to correct the sign using the sin of the angle between the tangent vector and the trajectory, the sign of the tangent of the angle and other similar stuff, but still i'm looking at some anomalies:
I'm pretty sure that those anomalies came from the fact that the graph is not a proper function and that the solution lies on the angle of the tangent vector, but still something is missing.
Any advice will be really appreciated,
thank you.
Alessandro

Answer 1

To track a 2D curve, you should be using an expression for the curvature that is appropriate for general parametrized 2D functions.
While implementing the equation from Wikipedia, you can use discrete differences to approximate the derivatives. Given the x and y coordinates, this could be implemented as follows:
% approximate 1st derivatives of x & y with discrete differences
dx = 0.5*(x(3:end)-x(1:end-2))
dy = 0.5*(y(3:end)-y(1:end-2))
dl = sqrt(dx.^2 + dy.^2)
xp = dx./dl
yp = dy./dl
% approximate 2nd derivatives of x & y with discrete differences
xpp = (x(3:end)-2*x(2:end-1)+x(1:end-2))./(dl.^2)
ypp = (y(3:end)-2*y(2:end-1)+y(1:end-2))./(dl.^2)
% Compute the curvature
curvature = (xp.*ypp - yp.*xpp) ./ ((xp.^2 + yp.^2).^(1.5))
For demonstration purposes I've also constructed a synthetic test signal (which can be used to recreate the same conditions), but you can obviously use your own data instead:
z1 = linspace(2,1,N).*exp(1i*linspace(0.75*pi,-0.25*pi,N))
z2 = 2*exp(-1i*0.25*pi) + linspace(1,2,N)*exp(1i*linspace(0.75*pi,2.25*pi,N))
z = cat(1,z1,z2)
x = real(z)
y = imag(z)
With the corresponding curvature results: