В этой статье будут рассмотрены некоторые проблемы и размышления, связанные с довольно интересной задачей коррекции/восстановления автомобильного трека на основе данных, полученных от MEMS датчика и навигационного приемника. Эта задача содержит много различных аспектов, наиболее интересный из которых - геометрический. Статья носит повествовательный стиль изложения, соответствующий анализу накопленных данных и наблюдений

Используемое оборудование

Рассмотрим основной используемый инструментарий и ключевые неидеальности, вносящие основной вклад в погрешность измерения положения.

Главным компонентом является спутниковый навигационный приемник TAU1312, далее по тексту просто приемник. Общее описание принципа работы приемника можно почитать в статье GPS Compendium - U-blox.

Один раз в секунду (некоторые приемники позволяют повысить частоту обновления данных до 10 Гц ) приемник выдает пакет сообщений о текущем положении и дополнительную информацию. Минимальный набор данных (RMC строка) содержит следующую информацию:

Широта
Долгота
Скорость
Угол направления движения относительно направления на север – путевой угол

Также в пакете данных содержится информация о высоте, количестве спутников, используемых в работе, оценка точности определения положения и многое другое.

Перечислим наиболее существенные для нашей задачи факторы искажения данных приемника:

Переотражение спутниковых сигналов от зданий. Эффект проявляется в систематическом искажении трека при проезде рядом с высокими зданиями. Трек «притягивается» к таким зданиям. Никаким усреднением это искажение не выпрямить
Путевой угол имеет невалидные или сильно искаженные значения при малых скоростях движения. Интересно отметить, что u-blox в отличие от SIMCOM и ALISTAR держит путевой угол при низких скоростях движения значительно лучше, почти как с гироскопом.
Временная задержка между моментом времени, соответствующим информации о положении в данных, и глобальным сигналом синхронизации по времени PPS (pulse per second). Эта задержка складывается из многих составляющих, таких как время передачи сигнала между спутниками и приемником , время вычисления, временная задержка фильтра данных (например, усреднение). В некоторых приемниках она может доходить до сотни миллисекунд. В добавок некоторые приемники (например, SIM68) могут залипать на одном месте при детектировании стоянки, что приводит к задержке при начале движения

Вторым инструментом является 6 осевой MEMS (Microelectromechanical system) датчик, далее по тексту просто датчик. В данной статье используются данные, снятые с помощью микросхемы lsm6ds3tr. Данная микросхема измеряет собственную угловую скорость вращения (гироскоп) и кажущееся ускорение $\overrightarrow{a}_{MEMS}=\overrightarrow{a}_{движения}-\overrightarrow{g}$ .Очевидно, что датчик должен быть жестко связан с корпусом автомобиля, поэтому плата должна быть прикреплена к автомобилю. Я использовал для крепления обычный двухсторонний скотч.

Наиболее существенные для рассматриваемой задачи факторы искажения данных:

Смещение показаний гироскопа. Если поставить MEMS сенсор неподвижным на стол, то он будет показывать, что микросхема вращается. Причем это вращение не связано с вращением Земли, так как значительно превышает его и не зависит от ориентации расположения датчика. Эти показания при покоящемся датчике и являются смещением.
Вклад смещения довольно легко уменьшить, так как оно достаточно стабильно по времени и в основном зависит только от температуры. Используя данное обстоятельство, можно в течение некоторого времени, когда автомобиль гарантировано стоит на месте, усреднить показания датчика и использовать их как оценку смещения.
Данный датчик имеет довольно большое смещение, многие конкуренты имеют в 3-6 раз меньше. Более того у меня получалось, что смещение по оси Z в два раза больше нормального (3dps по документации). Причем перепайка 3 разных образцов не дала изменений. Возможно это систематическое отклонение связано с не очень хорошей разводкой, так как неиспользуемые ноги я оставил неразведенными, что могло привести к механическим напряжениям в корпусе датчика.
Аналогичное смещение показаний присутствует у акселерометра. Но, в отличии от гироскопа, состояние покоя не позволяет оценить смещение, поскольку геометрия расположения сенсора относительно не известна. В заметках ST можно найти процедуру калибровки акселерометра.
Помимо смещений (аддитивная погрешность) датчик имеет погрешность коэффициента пересчета своих данных в физические величины (мультипликативная погрешность).
«Зашкаливание» показаний гироскопа, например, при торможении. Необходимо подобрать шкалу, для предотвращения выхода показаний за рабочий диапазон. Я использовал шкалу +/- 2000 dps.
Несовпадение осей гироскопа и акселерометра и их взаимное влияние друг на друга (погрешность ортогональности). Эти характеристики обычно не приводятся в документации, или приводятся в крайне урезанном виде.

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

Зная тип транспорта, можно внести дополнительные ограничения в алгоритм, повысив качество фильтрации. Например, для автомобиля можно перейти в двухмерное пространство, рассматривая лишь движение по поверхности, ортогональной $\overrightarrow{g}$ . Более того, скорость почти всегда полностью направлена вдоль оси автомобиля, если отвлечься от проскальзывания колес и заносов. Поворот оказывает малое изменение в проекции скорости на ось автомобиля при движении, и, чем выше скорость движения, тем это отклонение меньше. Для человека или квадрокоптера подобное утверждение не верно - человек может сделать шаг в бок, а на летательный аппарат накладывается скорость движения ветра, которая никак не связ��на с ориентацией аппарата.

Последним ключевым элементом является микроконтроллер STM32F405RG и SD карта для записи данных. Я использовал контроллер с FPU, так как масштабирование величин и грамотное накопление мелких движений представляет собой отдельную задачу, требующую тщательного анализа, аккуратности и тестирования, на которую не хотелось отвлекаться.

Особенности обработки данных MEMS датчика

Первым шагом является выработка правил работы с кинематическими величинами такими как ускорение и угол поворота. Датчик выдает показания в системе отсчета связанной с корпусом микросхемы. Определение угловых скоростей датчика можно взять из его документации. Поэтому необходимо научиться определять связь между локальной системой датчика и лабораторной системой — системой, связанной с Землей. Математические преобразования будут сначала выражены на языке матриц вращения, затем будет описана их связь с кватернионами.

Заметим, что преобразование радиус-вектора и преобразование вектора скорости, и преобразование вектора ускорения осуществляется по одному и тому же правилу. Действительно, ускорение и скорость – это производные радиус-вектора, т.е. предел отношения разницы между векторами положений в двух близких точках, к разнице времен между ними. Разница векторов, являясь вектором, преобразуется как вектор, а времена двух положений не изменяются при повороте (к тому же все скорости много-много меньше скорости света, единственное неприятное следствие ОТО — неотличимость гравитационных сил Земли от ускорения). Поэтому правило преобразования универсально для всех этих величин, и является правилом преобразования вектора.

Замечу, что, так называемый, «вектор» угловой скорости вектором не является. Определение угловой скорости можно записать в виде:

$\overrightarrow{v}={M}_\omega\cdot\overrightarrow{r},$

где ${M}_\omega=\begin{bmatrix}0 & -{w}_z & {w}_y \\{w}_z & 0 & -{w}_x \\ -{w}_y & {w}_x & 0\end{bmatrix}$ .

Поскольку скорость и радиус-вектор преобразуются как вектора, то отсюда можно найти закон преобразования угловой скорости: ${M}_\omega^{'}=T\cdot {M}_\omega \cdot T^{-1}$ , где T - матрица поворота. Использование этого соотношения дает куда более согласующееся с желаемым определение угла поворота, чем использование для преобразования «вектора» угловой скорости обычного правила:

${V}_\omega^{'}=T\cdot {V}_\omega$

где .

Чтобы не раздувать объем статьи еще больше и не загромождать ее формулами, отошлем читателя к AN5017 от NXP. В AN5017 представлены разные системы параметризации поворотов и их описание как на языке матриц, так и на языке кватернионов. В AN5022 рассказано как обновлять матрицу/кватернион вращения на основании данных гироскопа.

Изначально я реализовал обработку данных на основе матрицы поворота, используя линейное приближение:

${T}^{'}=T\cdot {T}_x(w_P\cdot dt) \cdot {T}_y(w_R\cdot dt) \cdot {T}_z(w_Y\cdot dt)$

где ${T}_i(\phi) -$ матрица поворота на угол $\phi$ вокруг оси i.

Данный алгоритм работал неплохо, но давал при резком торможении значительную ошибку, что выражалось в остаточном ускорении на уровне 0.05 g в плоскости движения после остановки. А это значительное ускорение. Можете оценить как далеко за минуту уедет автомобиль с таким ускорением. Описанную проблему оказалось легко повторить на столе, для этого достаточно приподнять плату за один край и отпустить. Остаточное ускорение после падения дает ошибку, которую необходимо минимизировать.

Первое что было сделано — была поднята частота опроса датчика с 500 Гц до 833 Гц. Повышение частоты дало заметное улучшение.

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

усреднить все возможные перестановки произведений матриц (их 3!=6 штук)
использовать для разложения общий вид матрицы поворота AN5021 формула (1), где вектор вращения задается "вектором" угловой скорости
использовать формализм кватернионов
Я выбрал 3 путь. Любопытно отметить, что с точностью до членов второго порядка по dt 1 и 3 пути совпадают (2 путь не проверял). Дополнительно кватернионы дают вычислительное преимущество:

умножение матриц требует 27 умножений-накоплений, а умножение кватернионов требует 16 умножений-наколений. Следовательно, можно ожидать меньший вклад ошибок округления
Ортонормировка матрицы является более развернутой процедурой, чем нормировка кватерниона. Нормировку необходимо периодически проводить, чтобы эффекты округления не приводили к потере соответствия. Только ортонормированная матрица может быть матрицей вращения, и только нормированный кватернион может быть сопоставлен повороту
Хранение истории поворотов «в лоб» требует 4 числа, вместо 9

Повышение частоты съема данных и переход на формализм кватернионов позволил снизить остаточное ускорение менее, чем 0.01 g

Следующей важной задачей является вычисление угла поворота $\psi$ (обозначение соответствует AN5017). Формула 21 задает правило его вычисления. Этот угол определяет путевой угол движения, поскольку, как отмечалось выше, скорость почти всегда почти полностью или полностью направлена вдоль оси корпуса автомобиля.

Формула 21 наглядно получается, если прикрепить спицу к плате (вектор (1,0,0) до поворота). Угол поворота проекции спицы (тени при освещении сверху) на землю и даст искомый угол. Но это вычисление содержит особенность при , так как при этом проекция превращается в точку. Если поставить плату на торец, то вы обнаружите что показания угла поворота будут крайне нестабильными. В этом положении смысл имеет лишь сумма углов, как это указано в формуле 24. Действительно, легко убедится, что плату можно поставить на торец совершив любой наперед заданный угол поворота.

Решения этой проблемы в AN5017 я не нашел. Получается, что при монтаже платы на вертикальную часть корпуса автомобиля мы не сможем внятно определять путевой угол. Единственное решение, которое я смог придумать — это циклическая замена осей. Т.е. изначально определяется одна из трех координатных осей, которая максимально коллинеарна с $\overrightarrow{g}$ , затем на этапе вычитывания данных из регистров датчика производится одна из трех циклических замен:

$\omega_x^{'}=\omega_x, \omega_y^{'}=\omega_y, \omega_z^{'}=\omega_z$
$\omega_x^{'}=\omega_z, \omega_y^{'}=\omega_x, \omega_z^{'}=\omega_y$
$\omega_x^{'}=\omega_y, \omega_y^{'}=\omega_z, \omega_z^{'}=\omega_x$

Замена выбирается так, чтобы $g_z^{'}$ была максимальной. Понятно, что выбор типа перестановки осуществляется однократно, поскольку плата и автомобиль жестко связаны, а автомобиль почти всегда едет без переворачивания. На столе этот прием позволил адекватно вычис��ять угол поворота платы, расположенной на торце, но на автомобиле я этот прием не проверял.

Последней темой в этом разделе будет обсуждение AN5023. Когда я поставил себе задачу коррекции трека, то не знал про эти замечательные app notes и пошел другим путем, более простым и менее общим. Поэтому, думаю, будет интересным сравнить эти два пути.

Ключевая проблема использования MEMS датчика состоит в том, что он измеряет ускорение на фоне большого естественного ускорения g. Поэтому небольшие погрешности в определении поворота через минуту-две сделают все заключения о местоположении невалидными. В связи с этим возникает естественное желание постоянно компенсировать накопившуюся ошибку. В AN5023 ускорение движения рассматривается как шум, на фоне которого выделяется и вычитается из показаний. На большом интервале времени это разумное допущение. Действительно, среднее значение ускорения за интервал времени $\triangle T$ выражается как $\langle \overrightarrow{a}\rangle =\triangle \overrightarrow{V} /\triangle T$ , поскольку скорость ограничена, то для больших интервалов времени среднее значение ускорения движения близко к 0. Поэтому среднее значение показаний акселерометра должно быть направлено по оси Z. Легко оценить гарантированный интервал времени, за который среднее значение ускорения движения будет меньше 0.01 g — около 5 мин. Только на таких масштабах времени для автомобиля ускорение можно рассматривать как шум с приемлемой точностью. Для ручных девайсов масштабы времен, конечно же, другие, поскольку там нет скоростей под 60 км/ч. Для рассматриваемой задачи шумы попадают в область 1/f шума, в которой статистические свойства не анализируются простыми статистическими методами. В AN5023 смещение гироскопа датчика моделируется суммой постоянного значения и гауссовского некоррелированного шума. Дополнительно температурная зависимость смещения противодействует статистическому описанию процессов, поскольку имеет систематический, а не случайный характер (прогрев салона, открывание дверей, включение кондиционера).

Также стоит критически рассмотреть использование магнетометра для коррекции поворота. Корпус автомобиля имеет собственное и наведенное магнитное поле, которое можно легко увидеть по компасу, поднеся его к автомобилю. Поэтому при расположении платы вблизи корпуса мы будем измерять сумму поля Земли и поля автомобиля. При повороте на дороге их векторная сумма очевидно будет изменяться, также крен автомобиля (скат дороги, неровная колея), наверное, тоже может дать смещение, которое приведет к «пролазу» $\overrightarrow{g}$ в поперечную плоскость движения. Для борьбы с этим осуществляется калибровка (в идеале алгоритм не должен подразумевать калибровку), но стабильность этой калибровки от температуры и времени не очевидна, поскольку магнитные свойства металла заметно зависят от температуры. Поэтому логичным видится предварительная отбраковка движений, после которой уже можно применять допущения, на основе которых строится методика в AN5023. Еще один потенциальный источник, создающий неоднородность в магнитном поле - крупные металлические конструкции городской застройки.

Я не пытался моделировать и параметризовать зависимость погрешностей датчика от времени, а просто реализовал «геометрический» фильтр верхних частот и процедуру оценки смещения гироскопа:

детектирование остановки (или хорошего прямолинейного равномерного движения). При детектировании остановки (с отсечением данных по краям временного отрезка) определяется оценка смещения гироскопа, а ось Z локальной системы (система автомобиля) совмещается с осью $\overrightarrow{g}$ , с сохранением угла поворота. Процедура совмещения векторов на языке кватернионов описана в AN5023 в разделе 3.4.
постоянная процедура Z-релаксации. В этой процедуре сначала определяются углы, на которые необходимо повернуть локальную систему относительно осей X и Y, чтобы ось Z совпала со средним накопленным вектором показаний акселерометра , а затем осуществляется поворот на доли этих углов. После производится поворот вокруг оси Z для сохранения значения угла поворота $\psi$
Интенсивность релаксации подбирается из соображений компромисса между подавлением реального ускорения движения автомобиля, и снижением вклада в вычисляемое ускорение от остаточного дрейфующего смещения гироскопа и накопленных ошибок. Я остановился на выполнении процедуры релаксации раз в 3 секунды с поворотом на 0.1 от полных углов компенсации.
Используемый простой алгоритм релаксации имеет недостаток - необходимость наличия ускорения в плоскости движения, которое компенсирует остаточное смещение гироскопа. Для борьбы с этой проблемой можно вводить оценку смещения, и динамически корректировать ее, подобно тому как это делается в AN5023. Но, поскольку значения смещений гироскопа на остановках отличались на 0 — 2 кванта (при выбранной шкале), то динамическая оценка смещения выглядит как превышение точности датчика, и ее стоит проводить при длительном движении (от 10 мин) без остановки, при котором другой альтернативы коррекции смещения не видно.

Переход в локальные координаты

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

Достаточно распространенная параметризация описания профиля Земли называется WGS84, в которой положение точки в глобальной декартовой системе (с началом отсчета в центре Земли) описывается системой:

$\begin{cases}X=(N(\phi)+h)\cdot cos(\phi)\cdot cos(\lambda) \\Y=(N(\phi)+h)\cdot cos(\phi)\cdot sin(\lambda)\\Z=(\frac{b^2}{a^2}N(\phi)+h)\cdot sin(\phi) \end{cases}$

, где $N(\phi)=\frac{1}{\sqrt{cos^2(\phi)+\frac{b^2}{a^2}}sin(\phi)}$

- отношение полуосей эллипса, равное приблизительно 0,996(6), $\phi$ - широта, $\lambda$ - долгота, h-высота.

Нас интересует процедура перехода в локальные декартовы координаты относительно какой-нибудь точки, например, первой валидной точки приемника.

Прежде всего нам необходимо сформировать векторы осей локальной системы. Направим ось X на север, поэтому ее вычисление логично связать с вычислением производной радиус-вектора по широте (для простоты примем h=0):

$\\\overrightarrow{e}_x=k\cdot (\frac{dX}{d\phi},\frac{dY}{d\phi},\frac{dZ}{d\phi}).$

k – нормирующий множитель.

Поправка на высоту (сотня метров) незначительна на фоне радиуса Земли, и ее учет лежит за пределами точности, с которой мы оперируем, поэтому принимаем h=0.

Ось Y направим на восток, поэтому ее вектор можно найти аналогичным образом, дифференцируя радиус-вектор положения по . Легко проверить, что полученные вектора осей ортогональны. Полученная плоскость XY строго говоря не ортогональна $\overrightarrow{g}$ . Гравитационное поле эллипсоида сложно рассчитывается, а поле реальной Земли еще сложнее. Тем не менее полученной точности преобразования вполне хватает, не она ограничивает качество коррекции. Ниже будет описан способ проверки этого утверждения.

Теперь ясна процедура преобразования из геодезических координат приемника в локальную систему. Прямое преобразование выглядит так:

По параметрам приемника определяются координаты точки в глобальной декартовой системе координат.
Вычисляется вектор смещения текущего положения $\overrightarrow{\triangle}$ относительно опорной точки $\overrightarrow{R_0}$
Вектор раскладывается по базису $\overrightarrow{e}_x,\overrightarrow{e}_y$ . Полученные координаты и есть искомые значения. Т.е. $x=(\overrightarrow{\triangle},\overrightarrow{e}_x), y=(\overrightarrow{\triangle},\overrightarrow{e}_y)$

Обратное преобразование из координат (x, y) в геодезические:

Строится вектор $\overrightarrow{R}=\overrightarrow{R_0}+x\cdot \overrightarrow{e}_x+y\cdot \overrightarrow{e}_y$
Определяются широта и долгота, соответствующие этому вектору $\overrightarrow{R}$

Корректность приведенной процедуры легко проверяется с помощью смещения координат трека - прибавления ко всем значениям x точек трека единицы, и сравнения исходного и смещенного трека в программе Google Earth Pro. Будет видно, что новый трек смещен относительно исходного на 1 метр на север. Аналогично проверяется смещение трека на восток.

Алгоритм коррекции

Задача: имея показания GNSS приемника и данные MEMS датчика, необходимо оценить траекторию движения автомобиля. Самый распространенный способ построения такого алгоритма, который можно найти в сети, — использование алгоритма Калмана. А именно, переход в локальные декартовы координаты и применение фильтра Калмана для каждой оси независимо.

Первая трудность, которая возникла при применении такого подхода — рассогласование данных по времени между приемником и датчиком. Если при обычном движении это рассогласование не так велико, но ощущается, то «залипание» при остановке сильно искажает логику фильтра. В сети можно найти статьи, в которых описывается решение этой задачи синхронизации данных.

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

Простого решения этих проблем я не нашел. К тому же хотелось использовать дополнительное ограничение, описанное в конце раздела «Используемое оборудование», - за исключением заносов и аварийных ситуаций автомобиль почти полностью движется вдоль своей оси, а значит угол $\psi$ можно отождествить с путевым углом минус угол поворота датчика относительно корпуса и минус угол дрейфа, связанный со смещением показаний гироскопа.

Ниже на рис. показаны графики значений путевого угла датчика и навигатора. Данные угла приемника неустойчивы при малых скоростях, поэтому имеются значительные скачки. На этом рисунке показания датчика уже скорректированы алгоритмом. Остаточный дрейф гироскопа достаточно мал, чтобы при пропадании спутникового сигнала путевой угол по датчику сохранял валидность достаточно долго — уход на 10 градусов за 5 -7 мин. При вычислении путевого угла по проекциям скорости движения, используя раздельную обработку данных по осям координат, на такое длительное сохранение валидности я бы не надеялся. Поэтому выглядит естественным разбивать движение на продольное — вдоль вектора, с углом-оценкой $\psi$ , и поперечное — перпендикулярное этому вектору. Мысль использовать не по-отдельности координаты XY, а связывать их через путевой угол, конечно же не нова. Например, я встретил подобную параметризацию в статье «Low cost relative GNSS positioning with IMU integration» CHALMERS.

Сопоставление путевых углов по приемнику и по датчику

Поэтому я остановился на алгоритме с блок-схемой, представленной ниже. Основная мысль состоит в сопоставлении точек траектории находящихся на достаточно далеком по времени расстоянии для оценки накопленных ошибок при вычислении траектории по показаниям датчика (априорный трек). Был выбран интервал между точками длительностью 6 с. Этот интервал достаточно большой, чтобы проблема временной синхронизации не играла значимой роли, и подходящий для размещения данных во внутренней RAM контроллера, используемых для вычисления априорного трека, и состоящих из усредненных пересчитанных в локальную систему координат показаний акселерометра с интервалом в 50 мс.

Сначала корректируется оценка путевого угла посредством обычного интегрального регулятора. Но коррекция осуществляется только при скорости движения выше определенного порога (был выбран порог в 20 км/ч). При детектировании остановки, ось Z устанавливается по вектору $\overrightarrow{g}$ , и обновляется оценка смещение гироскопа.

Следующим шагом оценивается угол поворота МЕМS датчика относительно оси автомобиля. Этот угол определяется как угол поворота вокруг оси $\overrightarrow{g}$ при переходе от системы датчика к системе автомобиля (с осью X вдоль его оси движения) и должен быть постоянен. Но поскольку взаимное расположение этих систем изначально не известно, то искомый угол необходимо оценивать. Оценка определяется так, чтобы среднее значение ускорения на ось автомобиля совпало с разностью скоростей на концах интервала движения, полученной по показаниям приемника, деленной на длительность интервала. Угол поворота обновляется с некоторым весом. Этот шаг коррекции имеет два аспекта:

При хорошем сигнале от спутников длина пути, вычисленная по координатам (учитывается лишь перемещение вдоль оценки путевого угла) и по скоростям движения совпадает в пределах пары процентов. При низком качестве сигнала рассогласование длин доходит до 10-15%. Поэтому имеет смысл масштабировать данные скорости приемника, поскольку он выдает не полную скорость автомобиля, а ее проекцию на плоскость движения, и, как мне кажется, именно это проектирование и вносит основную погрешность при плохом приеме сигнала. Было бы интересно получать данные о полной скорости автомобиля и самостоятельно вычислять проекцию, используя акселерометр как инклинометр, но данных о полной скорости движения в пакете данных я не нашел.
Поскольку искомый угол постоянен, то его регулярная корректировка — идеологически не правильный подход. Более того, при малой погрешности оценки угла, она будет давать квадратичный вклад (cos(x) вблизи 0 имеет нулевую производную). Более правильным будет следующий путь:
- грубо оценить угол описанным выше способом в качестве первичной оценки
- на поворотах при хорошем качестве спутниковых сигналов уточнять оценку угла по центростремительному ускорению (угловая скорость известна по гироскопу, линейная скорость — по ГНСС и акселерометру). Поворот еще интересен тем, что дает возможность оценить текущую скорость движения, используя это же соотношение между ускорением и скоростями в случае долговременной потери GNSS сигнала, когда накопленные ошибки уже не позволяют надеяться на вычисленное по ускорению значение
- рассогласование разницы скоростей на концах интервала между вычисленной по датчику и по приемнику приписывать не ошибке вычисления угла, а смещению ускорения, вызванным накопленной ошибкой вычисления поворота. И использовать это смещение для построения оценки трека

Но я ограничился простым обновлением оценки угла поворота датчика относительно корпуса.

После вычисления динамики скорости движения вдоль трека осуществляется коррекция смещения скорости (скорость движения в начальной точке рассматриваемого интервала времени). Для этого минимизируется рассогласование расстояний между конечными точками трека, полученных по вычислениям и по приемнику. Учтя это смещение получается новая оценка трека.

Последним этапом является применение деформации к треку, сближающей положения последней точки трека согласно вычислениям и согласно данным приемникам. Используются два типа деформации — продольная и поперечная. Продольная деформация удлиняет каждый шаг пропорционально скорости движения и пропорционально сонаправленности вектора перемещения с вектором рассогласования. Поперечная деформация также пропорциональна модулю вектора перемещения и направлена перпендикулярно ему. Коэффициенты поперечной и продольной деформации отличаются.

Не вдаваясь в дальнейшую детализацию, представлю некоторые результаты. Красный трек — данные от приемника, синий — после коррекции.

Для алгоритма коррекции помимо улучшения данных также важно не портить их, когда имеется хорошее качество приема спутникового сигнала. Ниже приведена часть трека, снятого с расположенной под лобовым стеклом платой

Сравнение треков с хорошим качеством спутникового сигнала

Заключение

Написание данной статьи ставило перед собой две цели. Первая — поделиться эстетическим удовольствием от применения геометрии к практической задаче. Вторая - проиллюстрировать борьбу между универсальностью и практичностью. Одна из основных задач инженера состоит в поиске подходящего компромисса между этими двумя силами. Фильтр Калмана в том рафинированном виде, в котором его описывают в подавляющем количестве пособий, как мне кажется, не особо подходит для данной задачи. Для его полноценной реализации требуется использование дополнительных построений. Например, в книге «Основы траекторной обработки радиолокационной информации» Коновалова А. А. описывается подход разделения траектории на маневры, каждый из которых обрабатывается подходящим фильтром Калмана, после чего уже отдельный алгоритм определяет наиболее удачное разбиение на эти маневры.

Геометрия и навигация