В настоящее время я нахожусь на втором сроке самоуправляемого автомобиля Udacity Nanodegree. Второй проект требует, чтобы мы реализовали Unscented Kalman Filter (UKF) на C++ и нашли оптимальные значения для некоторых параметров шума.

Первая задача (внедрение фильтра) — это в основном головоломка из того, что мы узнали и отработали на занятиях. Вам нужно знать, как организовать код:
 – читать данные
 – предсказывать следующее состояние (учитывая временной шаг)
 – обновлять наше мнение о состоянии (учитывая датчик data)
- Повторять, пока есть доступные данные

И это все!

Что ж… Есть несколько соображений, которые следует принять во внимание, даже если это звучит слишком прямолинейно:
- Проверить деление на ноль
- Нормализовать углы (всегда)
- Предварительно вычислить столько, сколько сможете
- Как обновить, когда входящие данные LIDAR.

Последнее очень важно, так как во время занятий они объясняют, как сделать шаг обновления для данных RADAR, но ничего не говорят о LIDAR. Мой логический вывод был следующим:
p = В последнем проекте нам пришлось использовать якобиан, чтобы справиться с нелинейностью данных RADAR (пожалуйста, поправьте меня, если я ошибаюсь).
q = Прилагательное UKF «без запаха» означает, что он берет «неприятную» часть от EKF.
r = Себастьян ненавидит якобианцев.
s = Ничего не было сказано о том, как обновлять, когда данные датчика поступают от ЛИДАР.

p ^q ^r ^s =› когда приходят данные LIDAR то обновлять так же как и в EKF

Таким образом, решив головоломку, следующая часть будет методом проб и ошибок с параметрами:
- Стандартное отклонение ускорения (std_a)
- Стандартное отклонение рыскания (std_yaw)
- Ковариационная матрица (P)

Из-за обязанностей в университете и моей жены у меня не так много времени, чтобы посвятить проекты Наностепени; Итак, я подумал: вместо того, чтобы работать за компьютером, чтобы найти эти ценности, я мог бы заставить мой компьютер работать на меня… (мысленное примечание: я определенно должен сделать то же самое с деньгами). Решение: ГА.

Генетический алгоритм

Простая часть, я всегда любил GA. Я создал очень простой, чтобы найти оптимальные (суб?) значения для этих параметров. Основные шаги:
1- Инициализация популяции
2- Вычисление приспособленности (и сортировка)
3- Выбор двух особей (родителей)
4- Создание потомства (путем скрещивания родителей)
5- Мутировать (с некоторой вероятностью)
6- Вычислить пригодность нового потомства
7- Интегрировать потомство в популяцию (путем случайной замены худших особей)
8- Сортировать популяцию
9- Перейдите к 3 и повторяйте до остановки

Инициализировать группу лиц.

Индивид — это просто объект, у которого есть атрибуты для значений, которые я пытаюсь найти (std_a, std_yaw, P). По сути, я создал новое решение класса с атрибутами: объект UKF и пригодность. Фитнес говорит нам, насколько хорошо решение; в моем случае я использую значение RMSE в качестве пригодности и кодирую GA, чтобы минимизировать его.

Чтобы инициализировать популяцию из N особей, я использовал некоторые «начальные» значения, которые я пробовал ранее и пробовал вручную. Они установлены в классе UKF, поэтому я просто добавил немного контролируемого случайного шума в функцию инициализации.

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

Вычислительная пригодность

Как уже упоминалось, пригодность «решения» — это значение среднеквадратичной ошибки, которое оно выдает с использованием данных датчика. Итак, чтобы вычислить его, я просто использую все данные (не так много), применяю процесс UKF (прогнозирование, обновление) и получаю значение RMSE в конце.

Цикл GA

Наконец, ядро ​​GA. Это цикл, который будет повторяться в течение нескольких «поколений». Есть несколько способов остановить выполнение GA, я выбрал самый простой.

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

Когда родители выбраны, начинается самое интересное ;) скрещивание

Учитывая простоту особи, скрещивание — это просто случайная передача частей «отца» и от «матери» к «ребенку». Для этого я с шансом 50–50 передаю «ген отца» или «ген матери» «ребенку».

Знаю, знаю… (0,5*0,5*0,5)*2 раза у потомства будет один и тот же «папа» или «мама» (опять же можно поспорить).

Идем дальше…

Мутация

Процесс мутации точно такой же, как и при инициализации: случайное добавление некоторого случайного шума к атрибутам.

Интегрировать потомство

Население в ГА обычно не растет, оно всегда остается неизменным. Для этого мы отбираем для нового потомства одних из худших адаптированных особей.

Сортируй и повторяй!

Отсортируйте людей по значению приспособленности (от самого низкого к верхнему) и повторите процесс выбора-скрещивания-мутации-интеграции.

Вывод

Я запустил GA только один раз и получил следующие значения RSME для px, py, vx и vy:

0.06
0.08
0.27
0.14

Значительно ниже минимально необходимых значений Udacity: [0,09, 0,10, 0,40, 0,30].

Код находится здесь, вы можете свободно использовать его с собственной реализацией UKF, оптимизировать его и т. д. Буду рад услышать, если он был полезен.