Project

General

Profile

Atividade #1212

Meta #1230: Ter bons filtro de posição e estimatidor de velocidades

Estimar as matrizes P, Q e R do Kalman

Added by Gabriel Borges da Conceição over 1 year ago. Updated about 1 year ago.

Status:
Em andamento
Priority:
Normal
Target version:
-
Start date:
10/29/2019
Due date:

Description

Um dos principais problemas do nosso Kalman é que as matrizes de covariância estão chutadas, com elementos apenas na diagonal principal.

Esta tarefa destina-se a estimar essas matrizes de covariância, sendo:

R -> matriz de covariância do erro de medição.
P -> matriz de covariância do erro de estado.
Q -> matriz de covariância do ruído do processo.

Temos que fazer uma estimativa desse trio de matrizes pra cada uma das câmeras.

KFEstimateAguardandoInicio.png (20.4 KB) Preview Gabriel Borges da Conceição, 03/07/2020 04:49 PM

KFEstimateinter.png (27.7 KB) Preview Gabriel Borges da Conceição, 03/07/2020 04:49 PM

KFEstimateFim.png (28.7 KB) Preview Gabriel Borges da Conceição, 03/07/2020 04:49 PM

KFEstimateCalculando.png (20.6 KB) Preview Gabriel Borges da Conceição, 03/07/2020 05:00 PM

KFEstimateSemColocarNaPosiçãoExata.png (25.7 KB) Preview Gabriel Borges da Conceição, 03/07/2020 05:21 PM

Kfestimateaguardandoinicio Kfestimateinter Kfestimatefim Kfestimatecalculando Kfestimatesemcolocarnaposi%c3%a7%c3%a3oexata

History

#1 Updated by Gabriel Borges da Conceição over 1 year ago

  • Subject changed from Estimar as matrizes P e R do Kalman to Estimar as matrizes P, Q e R do Kalman

#2 Updated by Gabriel Borges da Conceição about 1 year ago

  • Description updated (diff)

#3 Updated by Gabriel Borges da Conceição about 1 year ago

Eu e o Antonio vamos trabalhar nesta tarefa. Vamos antes terminar a tarefa de refazer a visão orientada a objeto para depois atacar aqui.

Previsão de começar essa na metade da semana que vem.

#4 Updated by Gabriel Borges da Conceição about 1 year ago

  • Description updated (diff)

Temos algumas ideias pra fazer as estimativas, provindas de conversa com o major Renault.

Falando primeiro da matriz R. Teoricamente, para estimá-la temos que pegar um grande número de medições da posição (x, y, orientação) de um robô em campo e calcular o erro de cada uma dessas medidas com a posição real. Teríamos então um vetor 2D de erros de medida. Daí é só calcular a matriz de covariância desse vetor e no labview já tem uma função pronta pra isso.

O problema é que não temos como saber a posição real do robô a cada instante. Temos em mente a seguinte estratégia:

Colocar o robô parado "exatamente" embaixo da câmera e pegar, por exemplo, 200 medições. Calcular a posição média dessas medições e considerar que essa é a posição real. Daí fazer o erro entre cada uma das 200 medidas com essa média.

Depois, pegar mais 200 medições em cada uma das 4 extremidades da câmera (meio do campo, escanteio, lateral no meio do campo etc), pois saberemos a posição exata do robô nessas posições; não será EXATA pq talvez não coloquemos o centro do robô no ponto completamente certo, mas cremos que é uma boa aproximação. Para cada um desses 4 pontos com 200 medições fazemos o erro com a posição "real" (o ponto ficará em função da geometria do campo, então é algo automático).

Teremos então um vetor 2D com 1000 erros de medição. Basta calcular a matriz de covariância desse vetor e será a matriz R usada para aquela câmera no código.

Esse processo deve ser repetido em cada uma das 4 câmeras.

Além das imperfeições já citadas para essa estratégia, não tem como fazer algo parecido com o robô andando. Então a matriz de covariância do erro de medição R será estimada utilizando apenas erros de medição com a câmera pegando o robô parado.

#5 Updated by Gabriel Borges da Conceição about 1 year ago

Falando agora da matriz de covariância do erro de estado P.

Diferentemente das matrizes R e Q, que são fixas durante todas as iterações do código, para a matriz P nós fazemos apenas a estimativa para a primeira iteração, P_0|0. As próprias equações do Kalman fazem estimativas para a matriz P (a priori na predição e a posteriori na atualização). Ou seja, precisamos estimar a matriz de covariância do erro do estado inicial apenas.

Para começar, percebemos que o estado do robô na primeira iteração, x_0|0, está com tudo 0 no código. Ou seja, para qualquer robô, estamos dizendo que ele começa no ponto (0,0) com orientação 0 e com velocidades nulas.

Vamos mudar isso e colocar x_0|0 com a posição vinda da câmera e com velocidades nulas. Esse estado inicial tem erros na posição apenas, porque o robô realmente está parado assim que a gente começa a rodar o código. A matriz P_0|0 que vamos estimar é a matriz de covariância do erro desse estado inicial e, como foi dito, tem erro apenas de medição. Então o que pensamos foi pegar o vetor 2D de erro de medições calculado para matriz R e adicionar em cada elemento do vetor valores 0 para os erros das 3 velocidades (v_tang, v_norm e v_ang), já que não há erro de velocidade no estado inicial e usar esse vetor 2D resultante para calcular a matriz de covariâcia P_0|0.

#6 Updated by Gabriel Borges da Conceição about 1 year ago

Por fim, a matriz Q é a matriz de covariância do ruído do processo. Então teoricamente temos que, a cada instante e com o robô andando, fazer o erro do estado gerado pelo Kalman (já com as matrizes R e P devidamente estimadas) com o estado real. Mas, mais uma vez não temos o estado real, pois não temos como saber a posição real do robô nem suas velocidades.

A ideia que o major Renault deu foi começar o Kalman com uma matriz Q carteada (ou nula talvez), fazer a predição e anotar o estado gerado; fazer a atualização e calcular o erro entre os estados gerados pelas duas fases. Calcular a matriz de covariância proveniente desse erro. Fazer isso por várias iterações até que a matriz Q comece a convergir (não sabemos se ela vai convergir, é uma esperança).

#7 Updated by Gabriel Borges da Conceição about 1 year ago

Um detalhe é que nosso Kalman não tem a componente opicional B_k.u_K das velocidades de controle. Então na predição, não mudamos velocidades, apenas posição. Acredito que isso deva interferir na estimativa da matriz Q.

Tiramos essa componente B_k.u_K da equação porque achamos que mais estava atrapalhando do que ajudando, já que não é, atualmente, nem um pouco confiável que o robô no campo execute as velocidades que estamos mandando.

Uma ideia que tivemos semestre passado e até fez o Kalman ficar razoavelmente bom no grsim foi pegar as posições vindas da câmera e derivá-las com o bloco de derivada do labview, que utiliza o método de tustin, e usar as velocidades geradas no vetor u_k.

Mas, por enquanto, vamos trabalhar sem essa componente.

#8 Updated by Gabriel Borges da Conceição about 1 year ago

Outro ponto importante que atrapalha o bom funcionamento do Kalman é estrutural.

Um desses problemas foi o gerado pela mudança e criação do while Read UDP (problema já descrito e em implementação na tarefa http://redmine.roboime.com.br/issues/1223).

Outro problema é que o major Renault disse que seria melhor que o Kalman trabalhasse a taxa constante (o dt ou intervalo que entra na matriz F). Ainda precisamos pensar sobre isso.

Outro problema, também relacionado ao dt ou intervalo, é que o timestamp dos frames está como instante em que o pacote chega ao nosso código (na verdade é o instante em que começamos a decodificar o pacote). Isso foi feito durante a larc 2018 pois, pelo o que entendemos, os computadores da visão estavam com horários um pouco defasados, então tinha informação que chegava depois com timestamp anterior e isso causava problemas no controle. Lembrando que o pacote vem com timestamp. E o ideal seria que o dt utilizado no Kalman fosse o intervalo entre as medições. Também precisamos pensar sobre isso.

#9 Updated by Gabriel Borges da Conceição about 1 year ago

O foco do nosso trabalho será começar com a estimação das matriz R e P, pois nelas temos ideias que, aparentemente, parecem que vão funcionar bem. E para isso, pensamos em começar fazendo uma VI separada do código principal que pegasse certo número de medições e fizesse as estimativas de R e P. Pra testar, vamos copiar e colar as matrizes no código.

Depois que fizermos elas, vamos partir para a estimação da matriz Q.

Lembrando que o grsim possui uma opção para ativar ruído gaussiano, além de opção de desaparecimento de robôs e bola. Isso vai ser importante para ir testando o resultado do nosso trabalho. Mas, claro que vamos sempre focar em testar no PIRF depois de terminada cada etapa.

#10 Updated by Gabriel Borges da Conceição about 1 year ago

Tudo que descrevi até agora foi falado para robôs. Mas para a bola vai ser a mesma coisa.

#11 Updated by Gabriel Borges da Conceição about 1 year ago

  • Parent task set to #1230

#12 Updated by Gabriel Borges da Conceição about 1 year ago

  • Assignee set to Gabriel Borges da Conceição

#13 Updated by Gabriel Borges da Conceição about 1 year ago

Estou montando uma VI que fica no while da visão para estimar as matrizes de covariância R e P da maneira que especifiquei nos comentários acima.

A VI que eu criei é um método de Vision e é uma máquina de estados. Tem que selecionar o id da câmera para a qual vai estimar as matrizes (vai ser uma par de matrizes R e P pra cada câmera), e clicar num botão de ok. Há um enum dizendo onde deve-se posicionar o robô. Ao terminar naquele ponto, o enum vai dizer onde posicionar novamente, depois que posicionar o robô, aperta ok de novo e assim vai até terminar os 5 pontos daquela câmera.

Eu fiz uma versão para o grsim (apenas fazendo a média como sendo o ponto real) e estou fazendo uma versão para o campo, tendo 5 pontos em cada câmera como já dito na tarefa. Essa versão do campo que estou fazendo contempla 4 câmeras como no PIRF e na LARC; por enquanto, fiz o código para uma dessas câmeras, falta copiar para as outras 3 e trocar alguns detalhes para cada câmera. Depois vou fazer uma versão para a RoboCup, que tem apenas uma câmera cobrindo o campo todo.

Assim que terminar a implementação da VI, vamos testar no PIRF e anotar os resultados.

#14 Updated by Gabriel Borges da Conceição about 1 year ago

O painel começa assim (após acionar o botão Estimate):
Current State "Aguardando início" e Next State "Abaixo da câmera" sinalizando onde você deve posicionar o robô.

Você deve escolher o local (grsim, LARC ou RoboCup), a quantidade de medidas que são pegas em cada um dos 5 pontos da câmera, o id da câmera para a qual serão estimadas as matrizes, além do id do robô que será utilizado para pegar as medições (a cor do robô será a que está indicada no team).

Depois disso e que posicionar o robô no local sinalizado pelo Next State, tem que clicar no botão "Continue"

Logo após ter clicado no botão "Continue", o que era Next State passa para o Current State (dizendo em qual ponto estão sendo medidas as amostras) e o Next State passa a ser "Calculando", indicando que as amostras de posição estão sendo pegas:

Após a quantidade de medidas que você especificou ter sido completada, acenderá o boolano "PonitFinished?", indicando que esse ponto terminou. Current State passa a ser "Aguardado próximo passo" e Next State passa a ser "Centro do gol esquedo" (no caso dessa câmera).

Após posicionar o robô no local indicado e clicar em Continue, o processo seguirá novamente no ponto indicado.

Depois que todos os pontos tiverem sido analisados, os States indicarão "Terminado", o booleano "CameraFinisehd" acenderá e as matrizes serão escritas em idicators que aparecem na tela, como abaixo:

Essas serão as matrizes que utilizaremos no código para essa câmera que foi analisada.

#15 Updated by Gabriel Borges da Conceição about 1 year ago

Identifiquei um problema.

No teste do qual foram tirados os prints acima, após cada ponto, eu posicionava o robô com a função Set Position do grsim, na qual digitamos as coordenadas e o robô se posiciona no ponto correto. Como consequência, encontramos valores que sabemos que são razoáveis (pelo menos analisando a diagonal principal de acordo com os desvios especificados para x e y no ruído do grsim).

Porém fiz outro teste parecido com o que faremos em campo, ou seja, posicionei o robô com o mouse na posição mais correta que eu conseguia. Fazendo desse jeito, as matrizes dão valores claramente não condizentes (levando em conta os desvios que colocamos nas coordenadas no ruído do grsim):

Vou adicionar o botão permitindo fazer o cálculo do erro utilizando as médias das posições de cada ponto para podermos comparar.

#16 Updated by Gabriel Borges da Conceição about 1 year ago

  • Status changed from Em andamento to Feedback

#17 Updated by Nicolas Oliveira about 1 year ago

Bom trabalho, concertou o erro? oq deu?

#18 Updated by Gabriel Borges da Conceição about 1 year ago

O erro que eu comentei aí na verdade é um erro de imprecisão de posicionamento. Colocando o robô não exatamente em cima da posição analisada, a estimação não fica boa.

Falta testar no PIRF e analisar como fica a estimação.

Antes disso, vou modificar um pouco o código para permitir fazer apenas uma média como sendo a posição real em cada um dos 5 pontos da câmera ao invés de utilizar o ponto real como sendo a coordenada exata de centro do gol, meio do campo e etc, por causa do erro de posicionamento que podemos cometer como citado acima.

#19 Updated by Nicolas Oliveira about 1 year ago

Gabriel Borges da Conceição escreveu:

O erro que eu comentei aí na verdade é um erro de imprecisão de posicionamento. Colocando o robô não exatamente em cima da posição analisada, a estimação não fica boa.

Entendi

Antes disso, vou modificar um pouco o código para permitir fazer apenas uma média como sendo a posição real em cada um dos 5 pontos da câmera ao invés de utilizar o ponto real como sendo a coordenada exata de centro do gol, meio do campo e etc, por causa do erro de posicionamento que podemos cometer como citado acima.

Acho boa essa ideia.

#20 Updated by Gabriel Borges da Conceição about 1 year ago

  • Status changed from Feedback to Em andamento

Also available in: Atom PDF

Go to top
Add picture from clipboard (Maximum size: 500 MB)