CODEWITHSUNDEEP
matlabmath
Rash
Rash

Reputation: 4336

Cost function based on inverse trigonometric functions

I have two points named as source and target. I know the coordinates of source (e.g, [-1 1]) and the problem is determining the location of target (e.g, [1 -1]). enter image description here

I have some measurements, one of them is the angle of the arrow pointing from target to the source (i.e, 135 deg).

In order to find the location of target, I define an error function to minimize; I'm using gradient descent algorithm, so I have a symbolic error function like;

syms xs ys xt yt
z(xs,ys,xt,yt) = theta - asin((ys - yt)/((xs - xt)^2 + (ys - yt)^2)^.5);

In which theta is the measured angle, 135 deg (for an ideal solution z, the error, would be zero, 135 - 135 = 0)

there are other measurements and I calculate the Jacobian of these error functions and minimize the error.

Anyhow, the problem is that theta in this example is 135 deg, but as we know the output for asin is [-90 90] and for this example it is asin(1/2^.5) = 45 deg. So for the right answer, my error function, z, won't be zero and I need to change my error function, what should I use?

I simply need an error function for my theta varying over [0 360], while asin and atan have the range of [-90 90] and I can't use if statements because of symbolic functions.

Upvotes: 2

Views: 328

Answers (2)

Luis Mendo
Luis Mendo

Reputation: 112679

Let phi denote the angle of the arrow from source to target. You want an error function that is non-negative and unimodal, with value zero for theta==phi only.

This criterion is satisfied by the function

z = (cos(theta) - cos(phi))^2 + (sin(theta) - sin(phi))^2;

or equivalently, using xs, ys, xt, yt instead of phi,

z = (cos(theta) - (xs-xt)/((xs - xt)^2 + (ys - yt)^2)^.5)^2 + ...
    (sin(theta) - (ys-yt)/((xs - xt)^2 + (ys - yt)^2)^.5)^2;

This error function has the following interpretation: if the angles theta and phi are represented as points in the unit circle, z is the squared length of the chord joining those two points.

Of course, the error could be defined in terms of length (instead of squared length) by simply applying sqrt to the expression above; and could be normalized between 0 and 1 by then dividing by 2.

Upvotes: 1

John Alexiou
John Alexiou

Reputation: 29244

Matlab defines sym/atan2(Y,X)

scr

If not you can create it with 

function [ theta ] = atan2( dy,dx )
%ATAN2 Four Quadrant Arc Tangent Function
%
    if dx>0
        theta = atan(dy/dx);
    elseif dx<0
        theta = atan(dy/dx)+sign(dy)*pi;
    else
        theta = sign(dy)*pi/2;
    end

end

Finally you get to use it like

z = theta - atan2(ys - yt, xs - xt);

as @LuisMendo pointed out.

update Wikipedia mentions that

atan2(y,x) = 2*atan(y/(sqrt(x^2+y^2)+x));

Upvotes: 2

Related Questions