Мое решение:
Строим функцию 3 переменных - (x,y,a) - координаты робота и угол его направления, соответствующую величине непохожести положения указанного её переменными на наше текущее положение, а затем примитивно минимизируем ее по совокупности переменных.
Функция будет составной, т.е. состоять из суммы слагаемых, каждое из которых соответствует 1 известному факту, который будет проверяться, например, функция непохожести положения видимого маяка №4 из камеры на положение, которое будет видно из (x,y,a).
------------------------------------------------------------------------------
Построим компоненту функции соответствующая видимому маяку:Пусть вектор B=(Bx,By,Bz) - абсолютные координаты маяка #i,
вектор V=(Vx,Vy,Vz) - в каких относительных координатах мы его видим;
Заранее расчитаем вектор nV=V/|V|=(nVx,nVy,nVz) - нормированный вектор V, а также длину вектора V, dV=|V|.
Теперь рассчитаем в каких локальных координатах W глядя из положения (x,y,a) будет находится маяк B:
W=(Wx,Wy,Wz)
Wx=cos(a)*(Bx-x)+sin(a)*(By-y)
Wy=cos(a)*(By-y)-sin(a)*(Bx-x)
Wz=Bz
Исходя из предыдущих рассуждений дальше нужно посчитать nW=W/|W|, и dW=|W|. После чего компонента функции выглядела бы так:
F[V,B](x,y,a)=|nW-nV|*Pn+|dW-dV|*Pd, где Pn, Pd - параметр оценивающий "маловероятность" ошибки в оценке направления и расстояния соответственно. Однако учитывая что dW и nW будут иметь "страшный и запутанный" вид, можно попробовать как-то упростить подход к поиску наиболее вероятного расположения.
Например сделать это можно запретив разные размеры погрешности при оценке направления и расстояния, и введя равные и независимые погрешности отклонения каждой локальной координаты.
Тогда компонента функции будет выглядеть так:
F[V,B](x,y,a)=|Wx-Vx|+|Wy-Vy|+|Wz-Vz|, или даже (чтобы не счиать производную модуля функции -
F[V,B](x,y,a)=(Wx-Vx)^2+(Wy-Vy)^2+(Wz-Vz)^2. При этом последнее слагаемое всегда константа, поэтому можно в нашей задаче минимизации ее отбросить.
Получаем компоненту функции соответствующую одному видимому маяку в виде:
F[V,B](x,y,a) = (cos(a)*(Bx-x)+sin(a)*(By-y)-Vx)^2 + (cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)^2.
Её производные:
F'x = -2*cos(a)*(cos(a)*(Bx-x)+sin(a)*(By-y)-Vx) + 2*sin(a)*(cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)
F'y = -2*sin(a)*(cos(a)*(Bx-x)+sin(a)*(By-y)-Vx) - 2*cos(a)*(cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)
F'a = 2*(cos(a)*(Bx-x)+sin(a)*(By-y)-Vx)*(cos(a)*(By-y)-sin(a)*(Bx-x)) - 2*(cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)*(sin(a)*(By-y)+cos(a)*(Bx-x))
Что мы видим? Мы видим что нет никакого желания оптимизировать такую функцию, ибо иначе нам придется сначала строить систему из трех уравнений с синусами и косинусами, а затем ее решать. Не знаю как вам, а я не рад.
Поэтому мы сделаем совершенно хамский по отношению к точным наукам, но вполне позволительный на практике шаг - переберем все углы "a" внешним циклом, а во внутреннем будем искать наиболее реальные (x,y), а уже по найденным стоить штрафную функцию соответствующую тройке (x,y,a), и из всех таких полученных троек и значений функции выберем с наименьшим значением штрафной функции.
Тогда мы можем сказать следующее:
F[V,B,a](x,y)=(cos(a)*(Bx-x)+sin(a)*(By-y)-Vx)^2 + (cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)^2,
т.е. свернув коэффициенты:
F[V,B,a](x,y)=(A1*x+B1*y+C1)^2 + (A2*x+B2*y+C2)^2,
где:
A1=-cos(a); B1=-sin(a); C1=cos(a)*Bx+sin(a)*By-Vx;
A2=-cos(a); B2=sin(a); C2=cos(a)*By-sin(a)*Bx-Vy;
или после перемножения и сложения:
F[V,B,a](x,y)=K20*x*x+K02*y*y+K11*x*y+K10*x+K01*y+K00,
где:
K20=A1*A1+A2*A2; K02=B1*B1+B2*B2; K00=C1*C1+C2*C2;
K11=A1*B1+A2*B2; K10=A1*C1+A2*C2; K01=B1*C1+B2*C2.
Далее считаем производные компоненты:
F'x=2*K20*x+K11*y+K10 и
F'y=2*K02*y+K11*x+K01Вот мы и получили компоненты для построения системы из двух линейных уравнений с двумя неизвестными. А такие решать мы уже умеем.
Осталось построить компоненту, соответствующую нашим знаниям о предполагаемом нашем месторасположении.
--------------------------------------------------------------------------------
Компонента соответствующая нашему знанию о предполагаемом месторасположении:
Пусть мы знаем что теоретически должны находится в координатах (tx,ty) и иметь угол направления ta, тогда
F[tx,ty,ta,a](x,y) = (ta-a)^2*Pa + (tx-x)^2*Pxy + (ty-y)^2*Pxy, где Pa - насколько точны наши данные об ожидаемом направлении, а Pxy - насколько точны наши данные об ожидаемых координатах.
После раскрытия скобок в компоненте, она будет выглядеть как:
F[tx,ty,ta,a](x,y) = K20*x*x+K02*y*y+K10*x+K01*y+K00,
где K20=Pxy, K02=Pxy, K10=-2*tx*Pxy, K01=-2*ty*Pxy,
K00=(ta-a)^2*Pa+(tx*tx+ty*ty)*Pxy
Производные компоненты:
F'x=2*K20*x+K10
F'y=2*K02*y+K01
-----------------------------------------------------------------------------------
Все, дальше для каждого случая перебираем переменную "а", составляем систему линейных уравнений F'x=0 & F'y=0, находим минимизирующие (x,y), рассчитываем штрафную функцию F(x,y,a), запоминаем ее с тройкой (x,y,a), далее из всех троек выбираем с минимальной штрафной функцией!
-----------------------------------------------------------------------------------
Если не убюсь с отладкой (хотя вроде и не такое отлаживал
да еще в спешке), то за выходные сделаю программу эмулятор-полигон для отладки, демку и может библиотеку, не исключено, что даже часть выложить успею.
