Futebol de Robos F180 » Inteligência

Trabalhar nesta tarefa!

Além do lifetime, foi refeito, na robocup, a parte de ignorar lados na visão. Isso também foi copiado para o código novo e se encontra na mesma VI.

O Nicolas informou que existe o lifetime dentro do Kalman e que já está implementado para os robôs. Durante a robocup, ele viu que tinha um erro na implementação pra bola e decidiu fazer na parte de decodificação de pacotes mesmo, como mostrado acima.

Devemos então retirar de onde atualmente está sendo feito e corrigir o que tem no Kalman.

Além disso, não sabemos se o lifetime dos robôs mantém a posição ou se mantem a trajetória. Devem ser realizados testes afim de averiguar isso.

Outro fato relacionado a isso é que as estimativas de velocidade estão sendo feitas pós Kalman e não sabemos como está o input e tratamento de velocidades no kalman. A ideia é fazermos essa estimativa com a derivada do labview (que usa método de tustin) antes do Kalman (logo, com as posições não filtradas) e passar isso como input.

Foi criada a branch LifeTime, a partir da dev, para trabalhar nesta tarefa.

Nesse teste do lifetime do robô, vc desligou a inserção do comando enviado pra ele no filtro? Pq se n isso pode ter influenciado e muito esse teste.

Em relação ao robô ganhar muita velocidade e ir para o infinito quando ele sai da visão, ainda não consegui identificar o que faz isso.

Consegui eliminar a hipótese de ser o controle, pois nada de controle está sendo usado no Kalman.

A ideia de inserir a velocidade é esse caminho mesmo!

Agora sobre o teste, refaça ele colocando probe nas variáveis de interesse.

Nicolas Oliveira escreveu:

A ideia de inserir a velocidade é esse caminho mesmo!

Agora sobre o teste, refaça ele colocando probe nas variáveis de interesse.

O problema de usar probe é que abrir uma dessas VIs faz o código ficar muito lento e as informações deixam de ser atualizadas.
Depois pensei em comparar as velocidades que vão sair do "novo Kalman" com as estimativas geradas no Team And Side, as quais já sabemos que apresentam resultado condizente com a realidade.

Meu foco não está sendo entender como funcionam ou são geradas as equações, já temos tudo pronto no nosso código. Estou focado apenas em entender o significado de cada matriz para poder aumenta-las e entrar com as velocidades medidas e ajustar seus valores de forma a melhorar o filtro.

O instrutor do vídeo mencionou que a parte mais importante é o ajuste dos valores das matrizes!

Trabalhei com o Antonio ontem e mudamos algumas coisas conceituais em relação a como queremos informar as velocidades.

A princípio queríamos pegar a posição direto das câmeras, usar a derivada de tustin do labview e colocar na matriz z a posição e a velocidade.

Porém, não achamos em lugar nenhum um exemplo em que há input de velocidades no Z. O curso que fizemos só colocava posição no Z. Então ficamos em dúvida se pode colocar ou se isso seria um erro conceitual. Vamos procurar o Major Renault para tirar essas dúvidas.

Só encontramos input de velocidades na matriz u. Então decidimos colocar nela. Essa parte da matriz u já estava implementada em nosso código, mas estava disabled. Bom, tivemos que mudar um pouco as contas com o u. Reescrevemos essa equação de acordo com o um material que achamos na internet.

Aqui temos também uma dúvida conceitual. Teoricamente, u é a matriz das velocidades de controle e não de medidas. Mas no curso que fizemos, por exemplo, o simulador executava exatamente a velocidade de controle. Então acaba sendo a velocidade que o robô está executando, que difere da medida pelo fato de haver imprecisão nas medições, como ruído e etc.

Escolhemos, para a programar de fato, pegar as velocidades saídas do bloco de derivada do labview e colocar na matriz u e analisar os resultados.

Para começar a testar e ter uma noção mínima do efeito das nossas modificações, fizemos o seguinte:
Na parte da comunicação, sobrescrevemos as velocidades do controle por todas nulas exceto v_tang cte = 0,5m/s.
A ideia é colocar o robô com orientação 0 no grsim, daí o robô terá somente vx.

Porém, mandar 0,5 m/s para o grsim não significa que isso será de fato executado, pois depende do computador e etc. Então apenas medir o erro entre a medida do kalman e 0,5 não significa muita coisa, pois não sabemos qual velocidade de fato o robô está executando.

O que fizemos foi comparar esse erro da estimativa do kalman com a estimativa feita pelo Nicolas (pega a posição na saída do kalman e deriva). Essa estimativa feita pelo Nicolas foi testada na RoboCup para a bola comparando com o gráfico do programa do juiz e mostrou ser bastante razoável. Além disso, na competição de gol a gol do nosso processo seletivo 2019.2, uma das equipes montou seu goleiro com a estratégia de apenas se posicionar de acordo com a trajetória da bola e teve desempenho muito bom.

Ou seja, não é perfeito, mas mostrou ser uma estimativa consideravelmente boa.

Então, se nosso Kalman tiver valores parecidos com essa estimativa, já podemos concluir que ele melhorou em comparação ao que é hoje.

Como eu disse, esse simples teste é apenas para conseguirmos mensurar minimamente o efeito das nossas alterações. Posteriormente vamos elaborar uma rotina de testes que possa nos dar confiabilidade para refinarmos melhor o filtro.

Mas, depois percebemos que o efeito da matriz u tem relação com aceleração e desaceleração, não tem influência quando se trata de velocidade constante.

Ou seja, fazendo apenas essa alteração no código, não devíamos ter melhora significativa no Kalman. De fato, não tivemos.

O nosso Kalman trabalha muito mal com variações de velocidade (o que acontece a todo instante com o robô em campo). Quando o robô atinge velocidade constante, passa um tempo e nosso filtro estima razoavelmente bem.

Bom, para isso temos que mexer nos valores das matrizes P e R. Essas matrizes têm elementos nulos exceto na diagonal principal.

A matriz P tem relação com a incerteza no seu atual estado. A matriz R tem relação com a incerteza medida que você está recebendo. Em ambas, se tiver total certeza, deve colocar valor 0 no elemento relativa ao que está analisando (x ou vx ou y...). Quanto maior a incerteza, maior deve ser o valor.

A matriz P tem efeito direto com a demora para reação quanto a mudança de velocidade.

A matriz R tem efeito direto na estimativa com velocidade constante.

Essas matrizes se correlacionam, mexer em uma causa influencia no efeito causado pela outra.

Só para ter noção da relação entre as ordens de grandeza dessas matrizes, nos exemplos do curso, ele usava 0,1 nos elementos de R e 1000 nos elementos de P.

No nosso código, os elementos de R estavam em torno de 0,1 e os de P, 1.

Então, fizemos o teste citado acima com os valores que estavam nessas matrizes e colocando 1000 na matriz P, a diferença na reação com a mudança de velocidade foi BRUTAL.

Pudemos perceber nesses vídeos que o incremento de P fez diferença brutal quanto à variação de velocidade. Depois, aumentar a confiança na medição (mudar R de 0.1 para 0.01, lembrando que menor é o número quanto maior for a confiança) também melhorou a reação à variação, porém a diferença foi mais suave e isso fez com que a imprecisão ficasse maior quando a velocidade era constante, provocando maior erro na medição de x também.

A implementação do filtro passa por esse tipo de análise, que é muito importante; e temos que chegar a valores que melhor nos sirvam. Nosso robô varia muito de velocidade em campo, então creio que o último caso seria melhor, mas para isso temos que montar uma rotina de testes e analisar.

No nosso código ainda não testei com esse incremento de P. Vou testar e postar o resultado.

Fiz o mesmo teste no Kalman antigo:

Podemos ver que demora muito mais para reagir à mudança de velocidade (o que acontece muito com o robô em campo), mas quando se trata de velocidade constante, não varia muito.

Lembrando que o "Kalman novo" por enquanto só foi acrescido de u (não faz muita diferença no nosso teste) e P com valores de 100 ao invés de 1.

Mesmo Teste no "novo kalman" somando 0.1 em P.


Não fez a diferença que eu esperava, até porque mesmo sem esse incremento a reação à mudança de velocidade já estava boa.

Nosso próximo passo é colocar velocidades na matriz z e consequentemente mexer em outras matrizes e analisar o resultado.

Queremos ter isso pronto até terça e já testar no pirf.

As alterações que fizemos no kalman e o teste montado estão na branch kalman-upgrade.

Estamos usando o bloquinho de derivada do labview. Eu percebi que a entrada dt dele só pega o valor informado na primeira iteração. Estávamos informando o tempo entre uma iteração e outra, mas apenas o primeiro valor estava sendo utilizado. Ele dá uma opção de initialize que atualiza o dt, mas faz a derivada com um x e seu anterior (e não com dois intervalos de diferença), então não podemos inicializar toda hora pois o método de tustin não estaria mais sendo utilizado.

Minha ideia é colocar 1/fps e inicializar apenas se o fps variar mais de 5 ou 10 unidades.

Lembrando que a diferença entre colocar 1/fps e a diferença de tempo entre uma iteração e outra é que pra calcular o fps, fazemos uma média das 100 últimas iterações.

Causei algum erro que faz com que quando eu desligue um robô da visão, o xk dele fique Nan quando eu ligo novamente.

Tenho que investigar

Em resumo, as melhorias feitas até agora foram:

1. Colocamos Vx, Vy e Vang na matriz da medição z. Antes, essa matriz tinha apenas x, y, orientação

2. Matriz P com valor 100.

3. Adição de 0,1 nos elementos da diagonal principal de P a cada iteração.

O resultado está representado pelo último gráfico postado aqui na tarefa. Daqui pra frente, o que nos resta fazer é testar valores para as matrizes P e R que cheguem ao melhor filtro.

Percebi um erro conceitual em nossas mudanças. O que fizemos foi basicamente inserir na matriz Z as 3 velocidades e mexer no tamanho das outras matrizes de acordo com isso.

Conceitualmente, temos que gerar essas velocidades derivando a posição de input (chamado de sample no nosso código). Mas, por distração, acabamos gerando essa velocidade a partir da posição contida na matriz xk. Na parte do código que fizemos isso, estamos derivando a posição tratada pelo kalman com uma iteração de atraso ao invés de derivar a posição que a câmera está nos mandando.

O que fizemos acabou sendo quase a mesma coisa que usar para velocidade de input a última velocidade contida em xk.

Temos que trocar isso para colocar no Z as velocidades derivadas a partir de pose do Z e analisar o efeito da mudança.

Em relação aos checklists resolvidos:

1.Corrigir robô não voltar à visão:

O problema é que quando o robô saía da visão e ficava sendo mantido pelo lifetime, sua matriz P começava a crescer até que chegava a +inf. Lembrando que quando o robô está fora da visão, o Kalman faz apenas x_k+1 = F . x_k e nada faz com a matriz P, apenas mantém o valor.

Não consegui identificar o que faz o P crescer assim, mas nessa situação (case do Use Observation False), passo a matriz identidade para o P, daí quando o robô volta à visão, sua matriz P está igual à da primeira iteração do código, I6x6.

2.Fazer o input de velocidades em Z derivando a partir de pose da sample e não de xk:
Feito e não houve mudança significativa visível.

3.Consertar o fato de o time amarelo estar influenciando no kalman do time azul (provavelmente deve ser apenas clonar alguma VI que desclonamos sem querer):
Realmente bastava clonar alguma VIs.

Em relação ao checklist "Consertar lifetime da bola", já descobrimos todos os problemas e resolvemos, mas isso foi feito na branch NewBallVision, a qual está sendo usada pra tratar os casos de aparecerem outras bolas em campo e etc.

Lembrando que tudo desta tarefa tem sido feito na branch KalmanZ e está tudo atualizado no git.

Gabriel Borges da Conceição escreveu:

Coloquei escala no eixo x (tirei auto-scale) para conseguir analisar melhor a reação à variação de velocidade:

Ao montarmos este gráfico e colocar aqui na tarefa, mencionei que o que faltava para ter uma noção melhor da precisão do kalman era conseguir saber qual velocidade o robô estava de fato executando, pois o que mandamos para o grsim não é executado na risca devido a processamento e etc.

Uma coisa simples que fiz foi mandar o robô andar a 0,5m/s e contar em quanto tempo ele percorreu metade do campo pra saber a velocidade que ele executou, ficou em torno de 0,3m/s, o kalman retornou valores em torno de 0,29m/s.