Определение координат робота по локальным координатам маяков

Классические подходы к определению глобальных координат обычно опираются на метод триангуляции или что-то в этом роде. Однако на практике часто вместо достаточной для триангуляции и точной информации, мы имеем избыточную, но неточную и поэтому противоречивую информацию. Например, мы знаем что расстояние между маяками 1 и 2 составляет 10м, но наши сенсоры выдали нам ответ, что расстояние до маяка 1 составляет 4 метра, а до маяка 2 расстояние 5 метров. В этом случае очевидно что мы имеем погрешность измерения расстояния до маяков минимум 0.5м. Если мы можем с хорошей точностью оценивать погрешность каждого известного нам параметра, то это в значительной степени может нам из массы неточной информации получить более-менее точные глобальные координаты.

Мы будем рассматривать ситуацию в которой у нас избыточная или достаточная информация, из которой мы можем с некоторой точностью назвать наши координаты и, возможно, направление.

Попробуем теперь понять, информация какого сорта может у нас оказаться:

• С погрешностью измеряемой в единицах угла:
  • Ориентация робота глобальная (компас, звезды); (измеренный угол - GAi, погрешность GAPi единиц угла);
  • Ориентация робота локальная (известные объекты на карте); (измеренный угол - Ai, погрешность APi единиц угла);

• С погрешностью измеряемой в единицах длины:
  • Расстояние от робота до известных объектов; (измеренное расстояние - Di, погрешность - DPi единиц длины)
  • Локальные координаты маяков относительно робота; (измеренные координаты - Xi,Yi, погрешность - ZPi единиц длины)
  • Глобальные координаты робота (GPS / другие готовые системы); (измеренные координаты - GXi,GYi, погрешность GZPi единиц длины)

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

Далее договоримся, что все погрешности ненулевые (нет абсолютно точных приборов дающих информацию с бесконечной точностью, даже если прибор не дает неправильного ответа, у него все равно есть шаг деления, и точность - минимум половина этого шага).

... Далее будет про минимизацию функции сумму взвешенных отклонений, вес каждого отклонения - 1/(погрешность измерения). ...