Bend radius from coordinates section

Dear Matlab Central,

I am going to try my best to explain the situation and what I want to achieve.

Currently I'm designing a dynamic vehicle simulation in matlab/simulink. For this simulation I'm provided with a set of x and y coordinates with time steps of 1 second. Now, for this simulation I want to find, real time, the corner radius that the vehicle is making during one drive cycle. So if the recorded vehicle is going in a straight line I want it to be zero and when, calculated out of the coordinates, the vehicle is in a corner I want to find that radius.

I am not sure how I can calculate this in simulink and I was wondering if any of you have had any experience in this or have a suggestion of what to try. I have been digging through some literature but I haven't found anything that fits my needs.

Thanks in advance! Kind regards, Michel.

PS. I will attach an m-file with the x and y coordinates. For every new coordinate (in X & Y) we are one second further down the drive cycle

[Eng. Fredius Magige]

Hi It might be better to have a certain range for practical application such as 0-5; 5.01-30; 30.01-60; 60.01-90 etc otherwise you ll endup with many corners. Otherwise three consecutive coordinates have high capability/chance to generate any corner/corners

[Image Analyst]

Isn't the radius of curvature of a straight line infinity, not zero?

[Michel de Jongh]

Hi Fredius, your absolutly right. That would make it more practical. Thanks.

Hi Image Analyst, that is true. I didn't thought of that.

Answer 1

[Mohammad Abouali]

You are looking to calculate Curvature. Here is the link with formula for you to calculate it:

https://en.wikipedia.org/wiki/Curvature

When it is straight line, the Radius is infinity, thus th curvature is zero.

[Michel de Jongh]

Hi Mohammad Abouali,

Thank you for your answer. Please correct me if I'm wrong:

I am using a cartesian coordinate system where the first coordinate is (in my example) [6.23, 52]. Now in order to calculate the curvature between this point and the second point [6.33,52] I need to apply this formula:

k (aprox)= | dy^2/dx^2 | = | (52-52)^2 / (6.33-6.23)^2 | = 0. I don't seem to grasp what I'm doing wrong here?

Or do I need to use the local expressions formula which states:

k = | x1*y2 - y1*x2 | / (x1^2 + y1^2)^3/2

k = | 6.33*52 - 52*6.23 | / (6.33^2 + 52^2)^3/2 = 1.2583e-10

Thank you.

[Mohammad Abouali]

the double prime means second derivative, while a single prime means first derivative with respect to time here. You could use Finite Difference Method or other similar method to calculate these derivatives.

Note that you have Latitude and Longitude. Those are geographic coordinates. You need to first convert them to a metric system, before using the equation shown in wikipage. If you have MATLAB Mapping Toolbox you can use the coordinate transform functionality of that. However, in your case you can also use deg2utm it should be accurate enough.

Something like this:

skip=5;
dt=1*skip;
[x,y,~] = deg2utm(Latitude(1:skip:end),Longitude(1:skip:end));

xp=(x(3:end)-x(1:end-2))/(2*dt);
yp=(y(3:end)-y(1:end-2))/(2*dt);
xpp=(x(1:end-2)-2*x(2:end-1)+x(3:end))/(dt^2);
ypp=(y(1:end-2)-2*y(2:end-1)+y(3:end))/(dt^2);

Kappa=abs(xp.*ypp-yp.*xpp)./((xp.^2+yp.^2).^(3/2));
R=1./Kappa;

% plot3(x(2:end-1),y(2:end-1),R);view(2)

plot(x(2:end-1),y(2:end-1));
hold on
idx=find(R<1000)+1;
plot(x(idx),y(idx),'r.')
legend('Route','Not straight','Location','SouthEast')

The skip is there to reduce the sensitivity a bot. Check the google earth KMZ file attached. Your path needs a bit of preprocessing before being analyzed. But also check the figure attached to see how the curves are highlighted. You can work on the parameters to fine tune it a bit and perhaps use a better smoothing algorithms than the one that I am using here, which is just skipping data.

[Michel de Jongh]

Thanks a bunch!

I only get an error when I'm trying to run the code.

Error: File: TEST.m Line: 3 Column: 19:

The construct "Latitude(...end...)" is ambiguous in this context, because "Latitude" cannot be ascertained to be either the name of a variable or of a function. To make it a variable, assign to it; to allow it to become a function at execution time, replace "end" with a call to LENGTH, SIZE, or NUMEL on the desired array.

[Mohammad Abouali]

You need to download deg2utm() from http://www.mathworks.com/matlabcentral/fileexchange/10915-deg2utm

That's not a regular function of MATLAB. It is provided from the MATLAB File Exchange.

Get that and see if that works.

Also you need to append your code that you provided earlier (Coordinates_Problem_Example.m) and include the Latitude and Longitude definition from your code at the beginning of the code snippet that I provided. They had lots of number and I couldn't fit it here. So I did not wrote it in that code. And that's what MATLAB is complaining. If you run that code, it comes to line 3 and then there is not Latitude/Longitude variable defined so it produces that error.

[cn1992]

Hello Mr Abouali,

Could you please post equations related to deg2utm code. I would like to understand the equations

Thanks in advance.

Answer 2

[Image Analyst]

Take a section of your curve coordinates (like, say, 9 to 21 points or so), and then use the FAQ to fit them to a circle. http://matlab.wikia.com/wiki/FAQ#How_can_I_fit_a_circle_to_a_set_of_XY_data.3F and get the radius.