Posição múltipla estima fusão

Eu tenho um sistema no qual tenho dois subsistemas separados para estimar as posições dos robôs. O primeiro subsistema é composto por 3 câmeras que são usadas para detectar marcadores que o robô está carregando e que gera 3 estimativas da posição e orientação do robô. O segundo subsistema é um sistema localizado no robô e medindo a velocidade nos dois pontos do robô. Ao integrar numericamente esses dois, posso obter uma estimativa da posição e orientação do robô (porque estou rastreando dois pontos ao mesmo tempo).

O primeiro sistema é menos preciso, mas o segundo sistema é desviado. O primeiro sistema fornece saída aproximadamente uma vez por segundo, enquanto o segundo fornece saída com muito mais frequência (100-200 vezes por segundo).

Suponho que deve haver uma abordagem melhor do que apenas redefinir a posição com a estimativa do primeiro sistema (como ela não é 100% precisa), mas também usar a posição acumulada do segundo sistema de sensor e fundi-la com os novos dados do primeiro sistema. Além disso, há uma questão de como fundir três estimativas do primeiro sistema? Deve haver uma maneira melhor do que a média pura, pois pode acontecer que as duas estimativas sejam exatamente iguais e a terceira seja completamente diferente (o que significa que provavelmente está mais errado)?

Você tem algum algoritmo de fusão para recomendar o uso em tal sistema? Conheço o filtro Kalman, mas estou tendo problemas para descobrir como usá-lo, pois os dois sistemas emitem dados em frequências diferentes.

Espero que a pergunta seja clara o suficiente, qual é a melhor abordagem para fundir as estimativas em uma estimativa mais correta e precisa?

obrigado

O que você está descrevendo é essencialmente um caso de livro didático para o uso de um filtro Kalman. Primeiro você precisa de uma etapa de previsão. Vamos supor que você está prevendo a pose do robô$(x,y,\theta)$, dada a estimativa de pose anterior e suas medidas de velocidade de alta frequência $(v,\omega)$, Onde $v$ é a velocidade linear e $\omega$ é a velocidade angular.

$P$ é a matriz de covariância 3x3 que representa a incerteza da pose do robô. $Q$ é a covariância de suas entradas (ou seja, quão barulhentas são essas medições de velocidade?) $F$ é o jacobiano do modelo de movimento em relação ao estado e $G$ é o jacobiano em relação às entradas, ou seja,

Agora você tem suas atualizações de correção menos frequentes, que na verdade medem o estado completo, tornando isso bastante simples, ou seja,

Onde $z_k$ é sua medida (da câmera) e $R$é a matriz de covariância associada a essa medida (provavelmente uma matriz diagonal). Essa medida é comparada com a medida prevista (que, no seu caso, é apenas a última estimativa de pose). Nesse caso simples, o ganho de Kalman é a proporção da covariância atual da pose em comparação com a soma da covariância da pose e da covariância da medida.

Para responder sua pergunta sobre as diferentes taxas, você pode executar sua atualização de movimento repetidamente até que sua atualização de previsão chegue. Por exemplo, pode acontecer que a atualização de movimento ocorra 100 vezes antes de você executar uma correção.

Você também perguntou sobre como lidar com três câmeras. A maneira mais fácil é apenas processá-los sequencialmente; basta aplicar três correções seguidas. Outra maneira é empilhá-los e executar uma única atualização. Você precisaria ajustar a etapa de atualização de correção para fazer isso dessa maneira.