最小二乘以极坐标表示在线

Question

我正在基于机器人本地化系统工作，说实话，我不确定这是数学问题还是实施问题。我正在尝试使用最小二乘拟合将线拟合到一组极坐标。该线以极性形式表示。我的主管给了我两个方程式，一个方程式确定角度，另一个方程式确定距原点的距离。 （参见图片）

https://i.stack.imgur.com/jUEZ5.png

我试图用C ++实现等式。

struct Pt {
    double d, angle;
};

// Polar coordinates of the points (-2, 1) and (5, 1) respectively.
std::vector<Pt> points = { { 2.2360679774997898, 2.6779450445889870 }, { 5.0990195135927845, 0.19739555984988075 } };

double a, r = 0;

double n = points.size();
double sumOfWeights = n;

double num1 = 0, num2 = 0, den1 = 0, den2 = 0;
for (int i = 0; i < n; i++) {
    double iP = points[i].d, iTheta = points[i].angle;

    num1 += iP * iP * sin(2 * iTheta);
    den1 += iP * iP * cos(2 * iTheta);

    for (int j = 0; j < n; j++) {
        double jP = points[j].d, jTheta = points[j].angle;

        num2 += iP * jP * cos(iTheta) * sin(jTheta);
        den2 += iP * jP * cos(iTheta + jTheta);

    }
}

a = 0.5 * atan2((num1 - (2.0 / sumOfWeights) * num2), (den1 - (1.0 / sumOfWeights) * den2));

for (int i = 0; i < n; i++) r += points[i].d * cos(points[i].angle - a);
r /= sumOfWeights;

然后我给它提供点（-2,1）和（5,1）的极坐标表示，结果是角度为0，距离为1.5，这是不正确的，因为线的角度应为pi / 2到原点的距离为1，对吗？

Answer 1

我在您的代码中看不到任何问题。

在您的示例中，您要查找的行具有等式 y = 1。

据我了解，这对应于等于0的角度，而不是pi / 2。您指出程序提供的角度为0。

Pi / 2是直线到原点的最近角度。

但是，距离计算 r 存在问题。在下文中，我将使用复杂的符号来简化分析：。

对于，您的距离计算对应于：

对于 a = 0，这对应于平均分的实数部分，在您的示例中为1.5。这是您的程序提供的。

通过用虚部代替实部，即用sin（。）代替cos（。），将获得正确值1。

但是，我不能保证这在所有情况下都能奏效。我邀请您在互联网上或与主管一起检查公式。抱歉，暂时无法做得更好。

注意：复杂的符号还可以简化计算。这是一个示例，即使我知道暂时不在您的优先级之内：

注意：第一步，我无法正确显示Tex方程。感谢Bob__的评论，我得以改进它。还是不完美...