Visualizando e Depurando um EKF

No momento, estou depurando e ajustando um EKF (Extended Kalman Filter). A tarefa é o rastreamento de pose de robô móvel clássico, onde os pontos de referência são marcadores de RA.

Às vezes me surpreendo como algumas medidas afetam a estimativa. Quando olho e calculo os números e matrizes envolvidos, posso descobrir como a etapa de atualização foi executada, o que e por que exatamente aconteceu, mas isso é muito tedioso.

Então, eu me pergunto se alguém está usando alguma técnica, truque ou visualização inteligente para ter uma idéia melhor do que está acontecendo na etapa de atualização do EKF?

ATUALIZAÇÃO # 1 (será mais específica e mostrará a primeira aproximação do que tenho em mente)

O que estou procurando é uma maneira de visualizar uma etapa da atualização de uma maneira que me dê uma ideia de como cada componente da medida afeta cada componente do estado.

Minha primeira idéia é plotar a medida e sua previsão juntamente com alguns vetores retirados da matriz K. Os vetores de K representam como o vetor de inovação (medição - previsão de medição, não plotado) afetará cada componente do estado.

Atualmente, estou trabalhando com um EKF, onde o estado é pose em 2D (x, y, ângulo) e as medidas também são poses em 2D.

Na imagem anexada (abra-a em nova página / guia para ver em resolução máxima), o vetor (em escala) K (1,1: 2) (sintaxe MATLAB para obter uma submatriz da matriz 3x3) deve ter uma idéia de como o primeiro O componente do estado EKF mudará com o vetor de inovação atual, K (2,1: 2), como o segundo componente do EKF será alterado etc. Neste exemplo, o vetor de inovação possui um componente x relativamente grande e está alinhado com vetor K (2,1: 2) - o segundo componente do estado (coordenada y) mudará mais.

Um problema nesse gráfico é que ele não mostra como o terceiro componente (ângulo) do vetor de inovação afeta o estado. O primeiro componente do estado aumenta um pouco, ao contrário do que K (1: 1: 2) indica - o terceiro componente da inovação causa isso, mas atualmente não consigo visualizar isso.

A primeira melhoria seria visualizar como o terceiro componente da inovação afeta o estado. Seria bom adicionar dados de covariância para ter uma ideia de como a matriz K é criada.

ATUALIZAÇÃO # 2 Agora, o gráfico possui vetores no espaço de estados que mostram como cada componente da medição altera a posição. A partir desse gráfico, posso ver que o terceiro componente da medição muda mais de estado.

Uma maneira muito informativa de visualizar o efeito das medições (para mim) é plotar o estado do robô (média, com elipse de covariância) antes e depois de cada medição. Em seguida, pegue os componentes individuais da medição (mancal, faixa para marcadores RA) e aplique-os separadamente para ter uma ideia.

Para fazer isso:

$a,b$$\theta$$[0, 2\pi]$

O rastreamento da covariância da hipótese anterior, estado medido e posterior é geralmente suficiente para descobrir se as equações de um EKF estão sendo aplicadas corretamente.

Boa sorte e não atualize sua pergunta com muita frequência. Em vez disso, volte com novas perguntas.

Algo que geralmente é feito é plotar as variáveis ​​de estado ao longo do tempo, bem como seus intervalos de 3 sigma. Os pontos em que esse intervalo diminui são as atualizações, nas quais você pode anotar a fonte das medidas envolvidas.

Além dos erros de implementação que devem ser verificados (não apenas equações erradas, mas também equações numericamente instáveis), o efeito das atualizações é diretamente afetado pela diferença entre o que é "esperado" e "medido" e suas respectivas incertezas. Portanto, você pode estar interessado em descobrir uma maneira de visualizar esse equilíbrio em termos da progressão do tempo no primeiro gráfico.