Всем привет.
Это моя реализация функции draw_lines() для Проекта 1 самоуправляемого автомобиля Nanograde компании Udacity с использованием фильтра Калмана вместо фильтра нижних частот.
Я использую реализацию фильтра Калмана в OpenCV, вот код, работающий с видео-вызовом:
Для того же проекта, использующего фильтр с низким прошлым, проверьте мой предыдущий пост.
Отказ от ответственности: я не являюсь экспертом в области фильтрации Калмана, поэтому вы можете столкнуться с некоторыми концептуальными ошибками или ошибками в коде в этом сообщении, имейте в виду. Это моя первая попытка использовать его в этом проекте, и я не тратил время на настройку параметров для достижения наилучших результатов (не стесняйтесь делать это самостоятельно и дайте мне знать!). Я также не буду углубляться в теорию фильтрации Калмана, в Интернете есть много очень хороших ресурсов по этой теме.
- Используйте глобальные переменные для хранения апостериорного состояния, оцененного с помощью фильтра Калмана
# 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 в левой строке
Аналогичные переменные состояния для правой строки.
Затем мы должны определить матрицы, которые алгоритм использует для вычислений в рамках физической модели и модели измерения:
Для физической модели:
- Матрица перехода
- Ковариационная матрица шума процесса
Для модели измерения:
- Матрица измерений
- Ковариационная матрица шума измерения
Наконец, мы рекурсивно применяем фильтрацию Калмана в два этапа:
- Шаг прогнозирования: здесь алгоритм использует физические законы движения для оценки или прогнозирования следующего состояния (т. е. где мы будем находиться в системе на следующем временном шаге); который будет шумным (не очень точным) из-за, например, того, что модель системы не точно представляет реальную.
- Шаг коррекции: алгоритм использует измерение, сделанное в текущий момент времени, которое также является зашумленным из-за ограничений точности и правильности датчика, и объединяет его с предыдущей оценкой состояния, чтобы получить новую скорректированную оценку, которая, как правило, будет лучше, чем использование только оценка, вычисленная с использованием физической модели или просто измерения датчика.
Заключение
Фильтрация Калмана широко используется в автономной робототехнике для навигации, отслеживания объектов и стабилизации, среди прочего. Он также используется в других областях и приложениях, таких как экономика, биология, химия, управление дорожным движением, аэрокосмическая навигация и т. д.
В таком проекте, как правило, мы хотели бы использовать более одного датчика (например, видеокамеру); добавить пару других типов датчиков и выполнить слияние датчиков с калибровкой Калмана, включая в то же время информацию о командах рулевого управления для получения гораздо более точных оценок состояния. Тем не менее, пока что этот конкретный пример работает очень хорошо, используя только обнаружение линий с помощью видеокамеры; теперь я потрачу некоторое время на настройку матриц, чтобы посмотреть, насколько можно улучшить результаты; если вы сделаете то же самое и найдете лучшие результаты, пожалуйста, дайте мне знать.
Спасибо за чтение.
Рауль.