Цепные фильтры Kalman

Моя команда строит робота для автономной навигации в условиях окружающей среды. Недавно мы получили новый встроенный датчик IMU / GPS, который, очевидно, выполняет расширенную фильтрацию Калмана на чипе. Он дает скорости тангажа, крена и рыскания, север, восток и вниз, а также широту и долготу.

Тем не менее, у нас также есть некоторые энкодеры, прикрепленные к нашим колесам, которые обеспечивают линейные и угловые скорости. Прежде чем мы получили этот новый датчик IMU / GPS, мы сделали свой собственный EKF, чтобы оценить наше состояние, используя датчики и некоторые другие недорогие датчики. Мы хотим использовать встроенный фильтр нового сенсора, но также включить наши кодеры в микшер.

Есть ли проблема с цепочкой фильтров? Я имею в виду, что мы будем использовать вывод встроенного EKF датчика IMU / GPS в качестве обновления нашего собственного EKF, так же как мы используем данные, считанные с кодировщиков, в качестве обновления нашего EKF. Это кажется мне разумным, но мне было интересно, что обычно следует делать в этом случае.

Вы можете использовать INS / GPS как обновления для выхода вашего первого EKF. Фактически это не цепочка, а просто обусловливание оценки на основе добавленной информации от INS / GPS.

Предположим, у нас есть следующие функции:

, P t + 1 | t = EKF_PREDICT ( x t , P t , u t ), для входов в виде состояния x , ковариации P и управляющих входов (оцененных по одометрии) u t .$x_{t+1|t}$$P_{t+1|t}$$x_t$$P_t$$u_t$$x$$P$$u_t$

а также

, P t + 1 | т + 1 = EKF_UPDATE ( х т + 1 | т , Р т + 1 | т , х т + 1 ). $x_{t+1|t+1}$$P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}$

Оценки от датчиков являются х т + 1 . У нас есть такие вещи, как:$\hat{x}_{t+1}$

$\hat{x}^{gps}_{t+1} = f(GPS)$

$\hat{x}^{map}_{t+1} = f(map)$

$\hat{x}^{ins}_{t+1} = f(INS)$

и т.д. для всех других способов оценки состояния робота. Так что запуск функции EKF_UPDATE для всех этих датчиков достаточно хорош.

Ваш цикл будет примерно таким:

за все время $t$

• Пусть - текущая одометрия / кинематическая оценка позы, а R u - шум этой оценки.$u_t$$R_u$

• , P t + 1 | t = EKF_PREDICT ( x t , P t , u t , R u )$x_{t+1|t}$$P_{t+1|t}$$x_t$$P_t$$u_t$$R_u$

• для всех датчиков ,$S$

  • Пусть х является оценка позе от этого датчика, и R S быть шум на этой оценке$\hat{x}^{S}_{t+1}$$R_{S}$

  • , P t + 1 | т + 1 = EKF_UPDATE ( х т + 1 | т , Р т + 1 | т , х т + 1 , R S ). $x_{t+1|t+1}$$P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}, R_S$

  • конечным для

• конечным для

Некоторые предостережения:

• Поскольку мы используем EKF, нет никакой гарантии, что оценка не зависит от порядка обновлений. То есть, если вы делаете INS, а затем GPS, итоговая оценка может отличаться от того, если вы обновите с помощью GPS, а затем INS. Обычно это не имеет большого значения, но фильтр требует значительно большей настройки.

• Пожалуйста, имейте в виду, что ваша INS имеет смещение и отклонения, которые могут повлиять на вашу долговременную надежность. GPS может помочь вам много здесь. Большая часть литературы одновременно оценивает смещение и дрейф в INS.