Всем привет.

Это моя реализация функции draw_lines() для Проекта 1 самоуправляемого автомобиля Nanograde компании Udacity с использованием фильтра Калмана вместо фильтра нижних частот.

Я использую реализацию фильтра Калмана в OpenCV, вот код, работающий с видео-вызовом:

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

Отказ от ответственности: я не являюсь экспертом в области фильтрации Калмана, поэтому вы можете столкнуться с некоторыми концептуальными ошибками или ошибками в коде в этом сообщении, имейте в виду. Это моя первая попытка использовать его в этом проекте, и я не тратил время на настройку параметров для достижения наилучших результатов (не стесняйтесь делать это самостоятельно и дайте мне знать!). Я также не буду углубляться в теорию фильтрации Калмана, в Интернете есть много очень хороших ресурсов по этой теме.

  1. Используйте глобальные переменные для хранения апостериорного состояния, оцененного с помощью фильтра Калмана
# Persistent variables. We need to store the last filtered state, sometimes we’ll have to drop calculations 
# from next frame but still need to draw something.
l_statepost_x1 = np.Inf
l_statepost_x2 = np.Inf
r_statepost_x1 = np.Inf
r_statepost_x2 = np.Inf

2. Объявить объекты фильтра Калмана, матрицы и вспомогательные переменные

Я также сделал это в глобальном масштабе:

# — — — — — Kalman object, matrices and aux variables for the left line
l_meas = np.array((2,1), np.float32) # For storing measurements
l_pred = np.zeros((2,1), np.float32) # For storing predictions
l_kalman = cv2.KalmanFilter(4,2) # Create Kalman object
# Initialize Kalman matrices
l_kalman.measurementMatrix = np.array([[1,0,0,0],[0,1,0,0]],np.float32)
l_kalman.transitionMatrix = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]],np.float32)
l_kalman.processNoiseCov = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]],np.float32) * (1e-5)
l_kalman.measurementNoiseCov = np.array([[1,0],[0,1]],np.float32) # [1,0],[0,1] is the default value anyway…
l_kalman.statePost = np.array([[218],[642.6], [0], [0]],np.float32)
# — — — — — Kalman object, matrices and aux variables for the right line
r_meas = np.array((2,1), np.float32) # For storing measurements
r_pred = np.zeros((2,1), np.float32) # For storing predictions
r_kalman = cv2.KalmanFilter(4,2) # Create Kalman object
# Initialize Kalman matrices
r_kalman.measurementMatrix = np.array([[1,0,0,0],[0,1,0,0]],np.float32)
r_kalman.transitionMatrix = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]],np.float32)
r_kalman.processNoiseCov = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]],np.float32) * (1e-5)
r_kalman.statePost = np.array([[1167.0],[674.6], [0], [0]],np.float32)

Обратите внимание, что я инициализирую переменную statePost фильтра эмпирически полученной оценкой x1 и x2 (я только что распечатал некоторые значения, найденные с помощью fitLine). Тем не менее, независимо от того, какие это значения, фильтр Калмана все равно должен улучшить оценку после нескольких итераций.

3. Проверьте мой предыдущий пост, чтобы узнать о следующих двух шагах…

  • 4. Группировать точки линий по наклону
  • 5. Применить фитлайн

6. Используйте Калмана, рисуйте линии

Код для левой строки, тот же подход для правой:

## — — — — — — — — — Draw left line — —— — — — — — — — — — — — — 
 l_m = l_vecy/l_vecx # Calculate new slope from normalized vector
 
 if l_m < -0.5 and l_m > -0.8: # Calculate x1 and x2 only if m is in range
 l_x1 = (l_y1 — l_pointy)/l_m + l_pointx
 l_x2 = (l_y2 — l_pointy)/l_m + l_pointx
# — — — — — — — — Apply Kalman filter
 #Prediction step
 l_predict = l_kalman.predict() # Not using the return value anyway
 l_meas = np.array([[np.float32(l_x1)],[np.float32(l_x2)]])
 
 #Correction step
 l_correct = l_kalman.correct(l_meas)
 l_statepost_x1 = int(l_correct[0])
 l_statepost_x2 = int(l_correct[1])
 
 cv2.line(img, (l_statepost_x2, l_y2), (l_statepost_x1, l_y1), (134, 186, 52), 10)
 
 ## — — — — — — — — — Draw right line — — — — — — — — — — — — — — 
 # (almost the same code from above, but for the right line)
 
 # (optional) Put some text
 cv2.putText(img,”Put some text of your own!”, \
 (50,50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,255,255),2,cv2.LINE_AA)

Несколько слов о фильтрации Калмана

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

Фильтр Калмана определяется просто как рекурсивная оценка; он вычисляет оценку текущего состояния, используя только предыдущую оценку состояния и текущее измерение.

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

  • позиция в ‘x’ точки_1 в левой строке
  • позиция в ‘x’ точки_2 в левой строке
  • скорость в ‘x’ точки_1 в левой строке
  • скорость в ‘x’ точки_2 в левой строке

Аналогичные переменные состояния для правой строки.

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

Для физической модели:

  • Матрица перехода
  • Ковариационная матрица шума процесса

Для модели измерения:

  • Матрица измерений
  • Ковариационная матрица шума измерения

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

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

Заключение

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

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

Спасибо за чтение.

Рауль.