基於反三角函數的成本函數

Question

我有兩個點，分別命名爲source和target。我知道源的座標（例如，[-1 1]），問題是確定目標的位置（例如，[1 -1]）。

我有一些測量，其中之一是從目標指向源的箭頭角度（即135 deg）。

爲了找到目標的位置，我定義了一個錯誤函數來最小化;我使用漸變下降算法，所以我有一個象徵性的錯誤函數，如;

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

在其theta是測得的角度，135 deg（對於一個理想的解決方案z，該錯誤，將是零，135 - 135 = 0）

還有其他測量和餘計算的這些誤差函數的雅可比和儘量減少錯誤。

總之，問題是theta在這個例子中是135 deg，但我們知道輸出asin是[-90 90]和這個例子是asin(1/2^.5) = 45 deg。所以對於正確的答案，我的錯誤函數z不會是零，我需要更改我的錯誤函數，我應該使用什麼？

我只是需要一個誤差函數爲我theta在不同[0 360]，而asin和atan有[-90 90]的範圍內，我不能使用，因爲象徵功能if語句。

Answer 1

讓phi表示從源箭頭到目標的角度。您需要一個非負數和單峯的錯誤函數，其值僅爲theta==phi。

這個標準是由函數

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

或等價滿意，使用xs，ys，xt，yt代替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; 

此錯誤有以下的解釋：如果角度theta和phi表示爲單位圓中的點數，z是加入這兩點的和絃長度。

當然，可在長度（而不是平方長度）方面通過簡單地應用sqrt上述表達式定義的誤差;並且可以在0和1之間歸一化，然後除以2。

Answer 2

Matlab的定義sym/atan2(Y,X)

如果沒有你可以用

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 

最後你使用它像

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

爲@LuisMendo指出，創建它。

更新 Wikipedia提到

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