Опыт работы с фильтром Калмана на примере NMEA данных

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


Используемые данные — c GPS-приемника в формате NMEA-0183, в часности сообщения GGA и VTG.


Фильтрация необходима по причине зашумленности GPS. Причины помех в GPS данных разные. Основные:


  • атмосферные помехи.
  • препятствия для сигнала.
  • положение орбиты GPS. Например, невысокое наклонение орбит GPS (примерно 55°) серьёзно ухудшает точность в приполярных районах Земли.

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


Дело в том что скорость, измеряемая оборудованием и передаваемая в сообщении VTG давала неправдоподобные показания (скачки и т. д.), которые крайне затрудняли задачи управления.
Поэтому было решено построить модель фильтр в Octave, и получив скорость как производную от GPS данных, представленных сообщением GGA, сравнить с оригинальными данными скорости из сообщения VTG.


Для удобства сравнения данные требуется вывести на один график.


С фильтрацией данных и их производных прекрасно справляется фильтр Калмана.


Применение фильтра для задач автопилота и курсовертикали является «классикой».


Поскольку я не специалист по теоретическим выкладкам — алгоритм работы фильтра здесь рассмотрен не будет. На эту тему есть обширные (и не очень) обзоры для всех уровней подготовки: от инженера-чайника до математика-гения с мехмата. Для читателей, незнакомых со всеми этими Predict, Update, и Invariant несколько ссылок:


https://habr.com/ru/post/140274/
https://habr.com/ru/post/166693/
https://ru.wikipedia.org/wiki/%D0%A4%D0%B8%D0%BB%D1%8C%D1%82%D1%80_%D0%9A%D0%B0%D0%BB%D0%BC%D0%B0%D0%BD%D0%B0


Первый этап работы состоял в написании фильтра в Octave (open-source близнец Matlab для Linux). Для начала матрица управления F мат. модели была выбрана простейшей (движение без ускорения):



Впоследствии ее можно модифицировать, наращивая потенциал фильтрации.
Второй этап — перенос фильтра в вычислитель (PAC контроллер) на C++.


Первый этап работы — применение фильтра в Octave описан в этой статье.


Второй этап с С++ кодом, логированием отфильтрованных данных, передачей их в Octave и сравнением модели и реализации — возможная тема для следующей.


Описание стандартом GGA сообщения:


Описание стандартом VTG сообщения:

Два знака «--» в шапке — произвольные символы, зависящие от аппаратуры. Фильтровать будем Latitude и Longitude.


Математическая часть фильтра Калмана.


Сухая выжимка формул, на основе которых осуществляется реализация.

Обозначения:
Fk — матрица эволюции процесса/перехода/транзита, state-transition model.
Hk — матрица измерений/наблюдений, observation model.
Qk — матрица ковариации шума процесса, covariance of the process noise;.
Rk — матрица ковариации шума измерений, covariance of the observation noise.
K — усиление Калмана.
Xp — прогнозируемая (априорная) оценка состояния на основании прошлого состояния.
Xk — обновленная оценка состояния.
X(k-1) — обновленная оценка состояния предъидущего шага.
Pp — прогнозируемая оценка ковариационной матрицы, предсказание ошибки.
P — обновленное предсказание ошибки.
P(k-1) — ошибка (обновленная) предъидущего шага.
z- наблюдение (измерение) в текущий момент времени.


Предсказание:
Xp = Fk X(k-1); Предсказание состояния системы.
Pp = Fk
P(k-1) * F'k + Q; Прогнозируемое предсказание ошибки.


Корректировка:
K = Pp H' inv(H Pp H' + R); Вычисление усиления Калмана.
Xk = Xp + K(z — HXp); Обновление оценки с учетом измерения z.
P = Pp — KHPp; Обновление предсказания ошибки.


Реализация фильтра по формулам.


Функции KalmanX и KalmanY идентичны и отличаются только названием.


Движения по X и Y фильтруются независимо, на вход фильтр принимает очередное значение, матрицы Q, R, F и признак reset (поскольку данные фильтруются несколько раз с разными Q и R, для подбора оптимальных значений).


Основной скрипт (habrGGA.m) читает дамп данных из текстового файла, вызывает функцию фильтрации, описанную выше и строит графики сырых и отфильтрованных значений.


Данные поочередно фильтруются с тремя матрицами Q, при этом матрица R остается неизменной. (Можно так же оставить неизменной Q, и менять R). Сравнивая графики, подбираем оптимальные значения коэффициентов для матриц.


На первом графике выводятся отфильтрованные координаты, или траекторию:



Мы видим оригинальные (не фильтрованные) данные, и данные с тремя разными Q. R во всех трех случаях = 0,2.


Вот увеличенный первый, наиболее зашумленный участок:



Второй и третий графики — это отдельно проекции трека на X и Y координаты (Latitude и Longitude) в верхней части, и скорости в нижней:




Увеличенные графики скорости:



Оригинальная скорость (из VTG) представлена пунктирной красной линией, фильтрованные значения хорошо сглаживают скачки.


Читатели могут поэкспериментировать с другими значениями Q и R, изменив следующую часть кода (habrGGA.m):


 if j==1   Q = [ 0.01 0;
                  0   0.01];
            Q1=Q;     
            R=0.2; R1=R;
  elseif j==2   Q = [ 0.05 0;
                      0   0.05]; 
                      Q2=Q;
            R=0.2; R1=R;
  elseif j==3    Q = [1 0;
                       0  1];
                       Q3=Q;
            R=0.2; R1=R;
  end

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


Приложенные файлы.
habrGGA.m — основной скрипт (чтение данных, вызов функции фильтрации, вывод графиков)
KalmanX.m — фильтр Калмана (Latitude)
KalmanY.m — фильтр Калмана (Longitude)

Похожие публикации

AdBlock похитил этот баннер, но баннеры не зубы — отрастут

Подробнее
Реклама

Комментарии 5

      +1

      У вас инвариантый объект со стационарными шумами, вы зря усложняете и делаете нестационарный фильтр. То есть вам достаточно один раз посчитать оптимальный гейн и не надо потом на каждом шаге его пересчитывать. Равно как и не надо пересчитывать матрицу ковариаций, она считается один раз заранее, если вдруг нужна.
      А так как матрицы Q и R вы берете не по априорным соображениям и не оцениваете из экспериментальных данных, а просто подбираете, то это уже и не оптимальный фильтр Калмана, а просто линейный стационарный фильтр с подобранными параметрами, фамилия Калмана из названия исчезает, и магия рассеивается. Вуаля! :)

        0
        Здесь приведен пример — для тех кто сталкнется с чем-то подобным в реальных задачах. Лог подбирался по наглядности. Шумы к сожалению есть разной природы — в моем случае особо досаждали шумы скорости из VTG, которые отлично видны на графиках. Штатное оборудование к сожалению с фильтрацией видимо справлялось не очень. Скорость из VTG следовательно брать было не желательно.
        0
        Более точный, и возможно динамически изменяемый подбор коэффициентов будет реализован при отладке на швартовных испытаниях.

        Ого, интересно, на каком таком корабле стоит автономно работающая (без инерциального навигационного комплекса) GPS навигация?

          0
          На корабле много чего есть, в том числе дублирующего, от метеостанции до компаса. Но приходящие NMEA сообщения от штатного оборудования (импортного в основном, и не дешевого) не устраивали заказчика. Шумы. Видимо их встроенные фильтра тоже не совершенны. И не настраиваемы.

        Только полноправные пользователи могут оставлять комментарии. Войдите, пожалуйста.

        Самое читаемое