Encadeamento de filtros Kalman

Minha equipe está construindo um robô para navegar autonomamente em um ambiente externo. Recentemente, adquirimos um novo sensor IMU / GPS integrado que aparentemente faz alguns filtros estendidos de Kalman no chip. Ele fornece velocidade de inclinação, rotação e guinada, norte, leste e abaixo das velocidades, latitude e longitude.

No entanto, também temos alguns codificadores conectados às nossas rodas, que fornecem velocidades lineares e angulares. Antes de adquirirmos este novo sensor IMU / GPS, criamos nosso próprio EKF para estimar nosso estado usando os codificadores e alguns outros sensores de baixo custo. Queremos usar o filtro no chip deste novo sensor, mas também incorporar nossos codificadores na mistura.

Existe algum problema com o encadeamento dos filtros? O que quero dizer é que usaríamos a saída do EKF no chip do sensor IMU / GPS como uma atualização para o nosso próprio EKF, assim como usamos os dados lidos dos codificadores como uma atualização para o nosso EKF. Parece-me razoável, mas eu queria saber o que geralmente deveria ser feito neste caso.

Você pode usar o INS / GPS como atualizações para a saída do seu primeiro EKF. Na verdade, isso não é encadeamento, mas simplesmente condiciona a estimativa com base nas informações adicionais do INS / GPS.

Suponha que tenhamos as seguintes funções:

, P t + 1 | t = EKF_PREDICT ( x t , P t , u t ), para entradas como estado x , covariância P e entradas de controle (estimadas por odometria) u t .$x_{t+1|t}$$P_{t+1|t}$$x_t$$P_t$$u_t$$x$$P$$u_t$

e

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

As estimativas de sensores são o x t + 1 . Temos coisas como:$\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)$

etc para todas as outras formas de estimar o estado do robô. Portanto, executar a função EKF_UPDATE para todos esses sensores é bom o suficiente.

Seu loop será algo como isto:

para sempre $t$

• Vamos ser o atual odometria / estimativa cinemática de pose, e R u ser o ruído nessa estimativa.$u_t$$R_u$

• $x_{t+1|t}$$P_{t+1|t}$$x_t$$P_t$$u_t$$R_u$

• $S$

  • $\hat{x}^{S}_{t+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$

  • final para

• final para

Algumas advertências são:

• Como estamos usando o EKF, não há garantia de que a estimativa seja independente da ordem das atualizações. Ou seja, se você faz o INS e o GPS, a estimativa resultante pode ser diferente da atualização com GPS e o INS. Geralmente, isso não é grande coisa, mas o filtro precisará de muito mais ajustes.

• Esteja ciente de que seu INS tem um viés e desvio, o que pode afetar sua confiabilidade a longo prazo. O GPS pode ajudá-lo muito aqui. A maioria da literatura estima simultaneamente o viés e a deriva no INS.