Отслеживание робот в круговом движении с использованием фильтра Калмана

Question

То, что я пытаюсь сделать:

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

Что у меня беда с:

• векторы состояния из фильтра Калмана сходится к центру окружности
• Калмана фильтр не удается найти истинные позиции
• Скриншот: Robot vs Kalman Filter

Мой код Реализация

• Gist Link

Answer 1

Я думаю, что есть две проблемы. Во-первых, вам не хватает матрица ковариации процесса Q. Если ваша модель перехода состояния не идеальна, это даст алгоритму намек на то, насколько неопределен прогноз. Большой Q заставит алгоритм больше полагаться на измерения. Попробуйте инициализацию

self.q = 0.001*self.f.dot(self.f.transpose()) 

, а затем в функции прогнозирования

self.p = self.f.dot(self.p).dot(self.f.transpose()) + self.q 

Другой вопрос заключается в том, что вы измеряете круговое (полярное) движение в декартовой плоскости. Вращение дает ускорение в X и Y, и в матрице F отсутствует. Я бы обновил матрицу F, включив в нее полную физическую модель, включая ускорение. Временной шаг (dT) также отсутствует и может быть добавлен в качестве аргумента.

class KalmanFilter(Filter): 
    def __init__(self, sigma, dT): 
    ... 
    self.f = np.array([[1, 0, dT, 0, dT*dT/2, 0], 
         [0, 1, 0, dT, 0, dT*dT/2], 
         [0, 0, 1, 0, dT, 0], 
         [0, 0, 0, 1, 0, dT], 
         [0, 0, 0, 0, 1, 0], 
         [0, 0, 0, 0, 0, 1]]) 

и, наконец, в своей основной функции

KF = KalmanFilter(sigma=1,dT=0.1) 

Я также увеличил сигмы на 1, чтобы получить более гладкое предсказание и снижение Р инициализации 1 от 999 визуализировать первоначальный выброс.

Вот результат: