Применение фильтра Калмана на роботе следящего за ногами

Question

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

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

Если можно рассчитать фильтр Калмана в 2-х измерениях, то я бы понял, что нужно проследить за тем, чтобы ноги следовали за ней, не смотря на то, что человек странно ходит (случайными движениями) -> В этом я сомневаюсь о том, как установить функцию матрицы состояний, может кто-нибудь сказать мне, как это сделать или сказать, где я могу найти больше информации об этом?

Благодарю.

Answer 1

Ну, вы должны прочитать о Kalman Filter. По сути, он оценивает состояние через его среднее значение и дисперсию отдельно. Государство может быть чем угодно. Вы можете иметь локальные координаты в вашем штате, но также и глобальные координаты.

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

Пример с глобальными координатами:

Предполагая, что у вас есть небольшая кубическая масса, которая может двигаться вперед со скоростью v. Вы можете просто смоделировать динамику только в локальных координатах, где ваше состояние s будет s = [v], которая является линейной моделью.

Но вы также можете включить глобальные координаты x и y, предполагая, что мы движемся только по плоскости. Тогда вы бы s = [x, y, phi, v]', Нам нужно phi отслеживать текущую ориентацию, поскольку куб, конечно, может двигаться только вперед относительно его ориентации. Давайте определимся phi как угол между прямым направлением куба и x-ось. Или другими словами: с phi=0 куб будет двигаться вдоль оси х, с phi=90° он будет двигаться вдоль оси Y.

Нелинейная динамика системы с глобальными координатами может быть записана как

s_dot = [x_dot, y_dot, phi_dot, v_dot]' 

с

x_dot = cos(phi) * v
y_dot = sin(phi) * v
phi_dot = ...
v_dot = ... (Newton's Law)

На этапе прогнозирования EKF (расширенный фильтр Калмана) вы должны использовать приведенные выше (дискретизированные) уравнения для прогнозирования среднего состояния на первом шаге и линеаризованные (и дискретизированные) уравнения для прогнозирования дисперсии.

Есть две вещи, которые нужно иметь в виду, когда вы решаете, что ваш вектор состояния s должен выглядеть так:

• Вы могли бы испытать желание использовать мой линейный пример s = [v] а затем интегрировать скорость вне фильтра Калмана, чтобы получить оценки глобальных координат. Это сработало бы, но вы потеряли бы удивительность фильтра Калмана, поскольку вы бы интегрировали только среднее значение состояния, а не его дисперсию. Другими словами, вы бы не знали, какова текущая неопределенность для ваших глобальных координат.
• Второй шаг фильтра Калмана, обновление измерения или коррекции, требует, чтобы вы описали выход вашего датчика как функцию ваших состояний. Поэтому вам, возможно, придется добавить состояния в ваше представление, чтобы вы могли правильно выразить свои измерения as z[k] = h(s[k], w[k]) где z измерения и w вектор шума с гауссовым распределением.