Определение координат робота по расстояниям до маяков, измеренным с одинаковым отклонением — различия между версиями
=DeaD= (обсуждение | вклад) (→Решение численным методом Ньютона) |
=DeaD= (обсуждение | вклад) |
||
Строка 19: | Строка 19: | ||
(zi-z)dzi = (xi-x)dx + (yi-y)dy + (zi-z)dz | (zi-z)dzi = (xi-x)dx + (yi-y)dy + (zi-z)dz | ||
− | + | При известных dzi три таких равенства представляют собой систему линейных уравнений относительно неизвестных dx,dy,dz. | |
+ | |||
+ | Примем d0=sqrt(dx^2+dy^2+dz^2), далее имея все эти выкладки запускаем следующий итерационный алгоритм. | ||
+ | |||
+ | # Инициализация x(0)=(x1+x2+x3)/3, y(0)=(y1+y2+y3)/3, z(0)=0; | ||
+ | # Шаг алгоритма №n (n>=1): | ||
+ | ## Вычисляем zi' исходя из координат x(n-1), y(n-1), z(n-1); | ||
+ | ## Находим их отклонение dzi=zi'-zi от реально измеренных zi; | ||
+ | ## Решаем систему линейных уравнений и находим dx,dy,dz, добавляем их к x(n-1),y(n-1),z(n-1) и получаем x(n),y(n),z(n); | ||
+ | ## Если d0 меньше некоторого epsilon, тогда останавливаемся и считаем найденные x(n),y(n),z(n) достаточно хорошим решением, которое невозможно уже значительно улучшить без серьезных времязатрат. | ||
+ | |||
+ | '''ВНИМАНИЕ!''' В решении есть проблема - непонятно почему для сходимости dz надо не добавлять, а отнимать от z(n-1). | ||
== Решение аналитическим методом == | == Решение аналитическим методом == |
Версия 12:19, 22 января 2009
Постановка задачи
Имеем в плоскости 3 маяка с известными координатами (x1,y1), (x2,y2), (x3,y3) и замеренные расстояния до них (z1,z2,z3) известные с точностью до неизвестного одинакового отклонения (z). Требуется определить наиболее вероятные координаты робота (x,y) и соответственно одинаковое отклонение (z), зависящее от смещения часов робота относительно часов маяков.
Источник задачи - система локализации на базе (ультра)звуковых маяков, которые могут быть рассинхронизированы по часам с автономным роботом.
Решение численным методом Ньютона
Решенее предложил mandigit
Запишем уравнение для расстояния ri от i-го маяка до робота:
zi=sqrt((xi-x)^2+(yi-y)^2)+z
Продифференцировав это равенство получим:
dzi = ((xi-x)dx+(yi-y)dy)/sqrt((xi-x)^2+(yi-y)^2)+dz = ((xi-x)dx+(yi-y)dy)/(zi-z)+dz, умножив на (zi-z), получим:
(zi-z)dzi = (xi-x)dx + (yi-y)dy + (zi-z)dz
При известных dzi три таких равенства представляют собой систему линейных уравнений относительно неизвестных dx,dy,dz.
Примем d0=sqrt(dx^2+dy^2+dz^2), далее имея все эти выкладки запускаем следующий итерационный алгоритм.
- Инициализация x(0)=(x1+x2+x3)/3, y(0)=(y1+y2+y3)/3, z(0)=0;
- Шаг алгоритма №n (n>=1):
- Вычисляем zi' исходя из координат x(n-1), y(n-1), z(n-1);
- Находим их отклонение dzi=zi'-zi от реально измеренных zi;
- Решаем систему линейных уравнений и находим dx,dy,dz, добавляем их к x(n-1),y(n-1),z(n-1) и получаем x(n),y(n),z(n);
- Если d0 меньше некоторого epsilon, тогда останавливаемся и считаем найденные x(n),y(n),z(n) достаточно хорошим решением, которое невозможно уже значительно улучшить без серьезных времязатрат.
ВНИМАНИЕ! В решении есть проблема - непонятно почему для сходимости dz надо не добавлять, а отнимать от z(n-1).
Решение аналитическим методом
Решенее предложил boez
Тут будет решение :)