Как правило, вопрос переполнения стека - это вопрос кодирования - здесь мы на самом деле не на этом уровне. Там могут быть другие сообщества, которые лучше подходят для этого вопроса.
При этом фильтр Калмана - не лучший способ обнаружить потенциальное столкновение. Есть две разные проблемы:
Оцените положение и скорость транспортных средств. Обычно для этого используется фильтр Калмана .
Предсказать, приведет ли траектория к столкновению. Решением этой проблемы обычно не является фильтр Калмана. Это проблема геометрии.
Мы из Калмана фильтруем оценки положения и скорости для обоих транспортных средств в одно и то же время. Мы строим отрезки на интересующий момент для обоих автомобилей. Сегменты начинаются с оценки текущего положения, а направление берется из вектора скорости. Длина отрезка является результатом того, сколько времени вы рассматриваете для обнаружения столкновений и величины индивидуальных оценок вектора скорости.
Тогда возникает вопрос о столкновении - пересекаются ли эти два отрезка? Фильтр Калмана не используется для обнаружения пересечения отрезков.