Наименьшие квадраты подходят по линии в полярном представлении

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, что неверно, поскольку линия должна иметь угол пи / 2 и расстояние до начала координат 1, верно?

Answer 1

Я не вижу проблем в вашем коде.

В вашем примере искомая линия имеет уравнение y = 1.

В моем понимании это соответствует углу, равному 0, а не пи / 2. Вы указали, что угол 0 - это то, что обеспечивает ваша программа.

Pi / 2 - угол ближайшей точки линии к началу координат.

Однако существует проблема для расчета расстояния r . Далее я буду использовать сложные обозначения для облегчения анализа: .

Для ваш расчет расстояния соответствует:

Для a = 0 это соответствует действительной части среднего значения очков, в действительности, 1,5 в вашем примере. Это то, что обеспечивает ваша программа.

Правильное значение 1 будет получено путем замены действительной части мнимой, то есть замены cos (.) На sin (.).

Однако я не могу гарантировать, что это будет работать во всех случаях. Я приглашаю вас проверить формулы в Интернете или у вашего руководителя. Извините, что не могу сделать лучше на данный момент.

Примечание: сложные обозначения также позволяют упростить вычисления. Вот пример, даже если я понимаю, что это не ваш приоритет в данный момент:

Примечание: я не смог правильно отобразить уравнения Текса на первом этапе. Благодаря комментарию Боба я смог улучшить его. Все еще не идеально ...