Цель статьи не в объяснении принципов Калмановского фильтра, а в его демонстрации на примере реальных (сырых) данных. Желающие могут модифицировать исходники и поэкспериментировать с алгоритмом, я надеюсь что моя работа поможет тем, кто столкнется с подобной задачей.
Используемые данные — 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 и сравнением модели и реализации — возможная тема для следующей.
Описание стандартом 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)
