Dados os modelos descritos nas seções anteriores, o procedimento de localização e ma- peamento consiste em obter estimativas da pose atual do robô ,ξk, bem como das poses
Figura 3.1: Trajetória percorrida pelo robô e conjunto de dados observados em cada pose indicativas de cada região, ξi,k, utilizando os conjuntos de dados providos pelos sensores. Ademais, em um contexto de estimação em sistemas híbridos, é também necessário estimar o vetor de probabilidades dos modos do sistema que são representados pelos índices de cada subregião do ambiente, isto é,
p(sk) = [Pr(sk = 1), Pr(sk= 2), ..., Pr(sk = nk)]T. (3.21)
Portanto, o processo de estimação consiste em obter uma estimativa para a f.d.p. a poste- riorip(xk, sk|y1:k). Sabendo que a variável sk está restrita a um conjunto finito de valores
discretos, o Teorema da Probabilidade Total permite que se escreva
p(xk, sk|y1:k) = nk
X
i=1
p(xk|sk= i, y1:k) Pr(sk= i|y1:k), (3.22)
ou seja, estimar (3.22) consiste em combinar a estimativa de vários filtros, cada um assu- mindo que um modo específico do sistema esteja em vigor no instante atual. Desta forma, a estimativa final é calculada por uma ponderação da estimativa de cada filtro por meio do vetor p(sk) de acordo com a probabilidade de suas saídas estarem corretas.
Reforçando aquilo que foi exposto no Capítulo 2.2 e na Seção 3.2, o algoritmo IMM mostra-se como uma boa opção para estimação de estados em sistemas a múltiplos modelos [49]. Por este motivo, este algoritmo foi escolhido como base para realizar a estimação de estados descrito por (3.17) e (3.19). Além disso, optou-se por incorporar às equações do IMM o algoritmo de estimação da MPT (matriz que rege as transições entre os modos do sistema) mostrado na Seção 2.2.2, visto que a hipótese de que essa matriz seja previamente conhecida raramente é verdade podendo levar a uma significativa perda de desempenho do filtro [111]. Portanto, além de prover as estimativas ˆxke ˆp(sk), o algoritmo é capaz também
de estimar recursivamente ˆΠk baseado em informações ruidosas extraídas das saídas dos
sensores. A seguir, descreve-se com mais detalhes cada passo envolvido no processo de estimação enfatizando as etapas de predição, observação e correção que são típicas em um contexto de estimação estocástica.
3.5.1 Predição das probabilidades dos modos
De acordo com o que foi exposto anteriormente, o passo de predição das probabilidades dos modos segue o modelo de evolução dado por uma Cadeia de Markov na qual a tran- sição entre os modos é determinada por uma matriz de probabilidades denominada MPT e representada por Πk, tal que
Πk = {πi,j(k)}, πi,j(k) = Pr{sk = j|sk−1 = i}, i, j ∈ [1, 2, ..., nk], ∀k ∈ N (3.23)
Então, definindo
ˆ
p(i)(sk) = Pr(sk = i|y1:k) (3.24)
e assumindo as seguintes condições iniciais
ˆ
p(i)(s0), ˆΠ0, i ∈ {1, 2, ..., nk}, (3.25)
calcula-se o vetor de probabilidades predito p(i)(s
k) através da seguinte equação
p(i)(sk) = nk X j=1 ˆ πj,i(k − 1)ˆp(j)(sk−1). (3.26)
3.5.2 Mistura das estimativas
O passo de mistura das estimativas é uma etapa típica do filtro IMM e que o distingue da maioria dos outros algoritmos de filtragem de sistemas a múltiplos modelos. Nesta etapa, as estimativas do passo anterior são combinadas de forma a gerar novas estimativas iniciais para o seu banco de filtros de Kalman a cada instante de tempo. Por causa deste passo de mistura de estimativas, o IMM apresenta requisitos computacionais que são lineares com relação ao número de modos do sistema. Uma ótima descrição da vantagem deste passo de mistura do filtro IMM é dada em [99] em que o autor enfatiza matematicamente a importancia desta etapa no desempenho do algoritmo.
Portanto, sejam ˆx(i)
k e ˆP
(i)
k , tal que i ∈ {1, 2, ..., nk}, o vetor de estados e sua matriz de
covariâncias associadas correspondendo ao filtro ajustado à região sk= i no k-ésimo instante
amostral. As equações para o passo de mistura são dadas por
x(i)k−1 = Pnk j=1πˆj,i(k − 1)ˆp(j)(sk−1)ˆx(j)k−1 p(i)(s k) , (3.27) P(i)k−1 = Pnk j=1πˆj,i(k − 1)ˆp(j)(sk−1) h ˆ Pk−1(j) + δ(i, j)i p(i)(s k) , (3.28)
δ(i, j) = xˆ(j)k−1− x(i)k−1 xˆ(j)k−1− x(i)k−1T . (3.29) 42
3.5.3 Predição das estimativas
Já foi mencionado nesta dissertação que o algoritmo de filtragem IMM aplicado ao pro- blema de SLAM em robótica móvel faz uso de um banco de estimadores relacionados a cada subregião da trajetória realizada pelo robô (modos do sistema). Cada estimador em particular utiliza uma técnica bastante conhecida na literatura denominada Filtro de Kalman Estendido (FKE). Portanto, a etapa de predição das estimativas de cada filtro segue basicamente as equações do passo de predição do FKE.
A etapa de predição do FKE utiliza o modelo descrito na Equação (3.18) para gerar uma estimativa predita para cada filtro correspondendo à subregião sk = i que é dada por x(i)k ,
considerando os dados disponíveis até o instante k − 1, isto é,
x(i)k = f (xk−1(i) , uk) (3.30)
em que uké o conjunto de dados proprioceptivos extraídos de Uk.
Nota-se claramente neste caso, que a função de processo em (3.30) não depende de qual subregião o robô está tomando as medidas de forma que os filtros de cada modo do sistema compartilham o mesmo modelo de predição.
A matriz de covariâncias de cada filtro também deve ser propagada através deste modelo como parte da etapa de predição. O FKE utiliza um processo de linearização em torno da última estimativa do vetor de estados x(i)
k−1 usando a matriz Jacobiana ▽xf da função f
avaliada em x(i)
k−1. Portanto a matriz de covariâncias deve ser propagada da seguinte maneira:
P(i)k = (▽xf )P (i)
k−1(▽xf )T + Qk (3.31)
em que a matriz Jacobiana ▽xf é dada por
▽xf =
∂f (x(i)k−1, uk)
∂x(i)k−1 (3.32)
Para considerar também os ruídos associados aos dados extraídos dos sensores proprio- ceptivos deve-se calcular também a Jacobiana ▽uf da função f com relação a uk, resultando
na seguinte matriz de covariâncias:
P(i)k = (▽xf )P(i)k−1(▽xf )T + (▽uf )Pu(▽uf )T + Qk, (3.33)
em que a matriz Pu representa as variâncias associadas às medidas dos sensores propriocep-
tivos e a matriz Jacobiana ▽uf é dada por
▽uf = ∂f (x
(i)
k−1, uk)
∂uk
3.5.4 Correção das estimativas
De acordo com o algoritmo do FKE, as estimativas preditas de cada filtro, x(i)
k , podem ser
corrigidas com base em um conjunto de medições ruidosas yk observadas no instante atual.
Em SLAM, o desempenho desta etapa está intimamente relacionado com um processo de ca- samento de dados pois as observações atuais são úteis para calcular uma estimativa da pose do robô somente se elas são realizadas de diferentes posições. Portanto, quando as observa- ções são recebidas por meio dos sensores exteroceptivos, elas devem ser associadas àquelas previamente armazenadas. Este processo consiste em procurar entre todos os conjuntos de dados Eipor estruturas similares aos dados provenientes de Dk.
Este processo de casamento de dados é efetuado, primeiramente, calculando-se um con- junto de observações preditas yk a partir das funções de medição hsk associadas a cada
subregião da trajetória conforme descrito na Equação (3.20), ou seja
yk= hsk(xk). (3.35)
Uma boa medida de proximidade pode ser calculada a partir do termo de inovação νk
dado pela seguinte expressão:
νk= yk− yk. (3.36)
Contudo, a incerteza associada a esta medida também deve ser determinada afim de que a similaridade entre a observação predita yk e a saída do sistema yk seja verificada de forma
consistente.
Os métodos mais comuns utilizados para resolver o problema de associação de dados baseiam-se em técnicas conhecidas como vizinho mais próximo [96, 119]. Uma medida es- tatística bastante comum na determinação de correspondência entre duas estimativas é o teste de Mahalanobis. Este teste fornece uma distância estatística entre duas estimativas de modo que o valor desta distância pode ser comparado com um limiar para validar a correspondência entre elas, isto é
d = νkTSk−1νk < dmin, (3.37)
em que Sk é a incerteza associada ao termo de inovação νk. Considerando que as estimati-
vas podem ser modeladas como variáveis aleatórias Gaussianas, a distância de Mahalanobis segue uma distribuição de probabilidade X2 e pode ser usado então para aceitar ou rejeitar
uma certa correspondência com um grau de confiança.
Conforme descrito na Seção 3.2 a determinação da verossimilhança de yk é uma etapa
básica na estimação da f.d.p. a posteriori p(xk, sk|y1:k) e o cálculo da expressão mostrada
em (3.10) pode ser usado na verificação do teste de Mahalanobis formulado em (3.37). Isto mostra que o processo de casamento de dados em SLAM já está, de certa forma, incorporado às equações do algoritmo de filtragem IMM.
Uma vez concluído o processo de associação de dados, a estimativa predita de cada filtro é corrigida de acordo com a correspondência verificada pelo teste de Mahalanobis.
Então, se existe uma correspondência entre yk e algum dado proveniente de Ei, tal que
i = [1, 2, ..., nk], a função de medição hsk pode ser escrita conforme em (3.20) e a estimativa
predita x(i)
k do filtro correspondente à subregião sk = i pode ser corrigida utilizando as
equações do FKE.
No algoritmo do FKE, o procedimento usual de correção consiste em agrupar em um único vetor de saída todos os dados correspondidos e processar todas as informações ao mesmo tempo. Contudo, este método pode mostrar-se computacionalmente ineficiente caso a dimensão do vetor de saída fique demasiadamente grande [120]. Uma alternativa a este método foi proposta em [121] em que os autores demonstram que os dados dos sensores podem ser processados sequencialmente caso suas medidas sejam descorrelacionadas. Por- tanto, considerando esta a situação, o processo de correção das estimativas preditas de cada filtro pode ser feito de forma progressiva à medida em que o processo de associação de dados determina as correspondências entre o conjunto de observações yk e os dados previamente
armazenados Ei. Este procedimento está melhor ilustrado na Figura 3.2.
Figura 3.2: Processo de associção de dados e correção sequencial das estimativas.
Ainda com relação à Figura 3.2 nota-se que quando não há correspondência com nenhum dos dados dos conjuntos Ei, ou seja, quando uma nova medida é tomada, ela não é incorporada
à etapa de correção e não contribui para o processo de atualização das estimativas.
Para concluir esta etapa, mostra-se uma breve explicação das equações utilizadas no pro- cesso de correção das estimativas. Esta etapa é realizada por uma matriz de correção comu- mente referida na literatura por matriz de ganho de Kalman Gk. Esta matriz de ganho realiza
uma soma ponderada entre as estimativas preditas e as observações yk e é calculada a partir
da covariância do termo de inovação Ske da matriz de covariâncias da estimativa predita Pk.
Desta forma, o processo de correção para as estimativas preditas do filtro correspondente à subregião sk = i é dado pelas seguintes equações
ˆ
x(i)k = x(i)k + Gkνk, (3.38)
ˆ
em que a matriz de ganho Gké dada por Gk = P (i) k ∂hsk(xk) ∂xk T Sk−1. (3.40)
3.5.5 Correção das probabilidades dos modos
Terminado o processo de correção das estimativas, uma etapa importante do algoritmo IMM é a atualização do vetor de probabilidade dos modos de operação do sistema a partir de yk. De acordo com a formulação do problema de SLAM proposta nesta dissertação, os
modos do sistema são representados por cada uma das subregiões da trajetória realizada pelo robô de forma que o vetor p(sk) denota as probabilidades dos índices associados à
cada subregião. Portanto, por meio do vetor de probabilidades corrigido ˆp(sk) é possível
determinar probabilisticamente de qual subregião o robô está tomando as medidas no instante atual.
O conhecimento acerca do vetor de probabilidades p(sk) a cada passo de estimação do
filtro levou à proposta de uma modificação no algoritmo convencional do estimador IMM que resultou em significativa melhora no desempenho para a solução do problema de SLAM. Os fundamentos desta pequena alteração serão melhor explicados na Seção 3.6.
Assim, com base no algoritmo do filtro IMM mostrado no Capítulo 2.2 as equações para a correção das probabilidades dos modos podem ser escritas da seguinte forma
ˆ p(i)(sk) = p(yk|sk= i, ˆΠk−1, y1:k−1)p(i)(sk) ci , (3.41) γp = nk X j=1 ˆ p(j)(sk), (3.42) ˆ p(sk) = [ˆp(1)(sk), ..., ˆp(nk)(sk)]T 1 γp , (3.43)
em que o termo p(yk|sk = i, ˆΠk−1, y1:k−1) é a verossimilhança de yk que já foi calculada
durante o processo de associação de dados e cié uma constante que não precisa ser determi-
nada.
3.5.6 Geração das saídas
Um vez calculadas as estimativas ˆx(i)k e ˆPk(i)para cada filtro correspondendo à subregião sk = i, tal que i ∈ [1, 2, ..., nk], bem como o vetor de probabilidades ˆp(sk), a estimativa
final dada pelo algoritmo IMM é determinada por uma soma ponderada de acordo com as 46
seguintes equações ˆ xk = nk X i=1 ˆ p(i)(s k), (3.44) ˆ Pk = nk X i=1 ˆ Pk(i)+ˆx(i)k − ˆxk ˆ x(i)k − ˆxk T . (3.45) 3.5.7 Atualização da MPT
Conforme já foi mencionado anteriormente, as transições entre os modos do sistema são governadas por uma Cadeia de Markov, cuja representação matemática é dada pela Matriz de Probabilidades de Transição (MPT). Esta matriz também é estimada online de acordo com o algoritmo Quasi-Bayesiano mostrado na Seção 2.2.2.
3.5.8 Incorporação de novos elementos ao vetor de estados
De acordo com o modelo descrito neste trabalho o vetor de estados contínuos de cada filtro x(i)
k correspondente à subregião sk = i é composto pela pose atual do robô, bem como
as poses quando da aquisição da dados anteriores. Cada pose é associada a uma região da trajetória e portanto, à medida que o robô se desloca novas regiões são definidas e a pose do robô neste instante deve ser incorporada ao vetor de estados de cada filtro.
A matriz de covariâncias P(i)
k desta nova pose deve também ser devidamente inicializada,
uma vez que ela é correlacionada com a estimativa da pose atual do robô e com as estimativas das outras poses que também fazem parte do vetor de estado do sistema. Ignorar o efeito desta correlação poderia levar a uma inconsistência no processo de estimação. Portanto, quando uma nova região é criada seguem-se as seguintes modificações para a matriz de covariâncias P(i)
k . O procedimento descrito a seguir é realizado para cada filtro seguindo
uma determinada subregião da trajetória.
Primeiramente, a matriz de covariâncias é aumentada com a incerteza associada à pose atual do robô. Depois, preenche-se o restante da matriz com as correlações cruzadas entre o restante dos elementos do vetor de estados. Assume-se que a matriz de covariâncias inicial é dada por P−
k e representada pela seguinte equação:
Pk−= Prr k Pkr1 · · · P rnk k Pr1 k Pk11 · · · P 1nk k ... ... . .. ... Prnk k P 1nk k · · · P nknk k (3.46) em que Prr
k representa as covariâncias relacionadas com a pose atual do robô, P jj
k , tal que
j ∈ [1, 2, ..., nk], representa as covariâncias associadas às poses de cada região da trajetória,
e as poses anteriores de cada região da trajetória e por último Pjk
k , tal que j, k ∈ [1, 2, ..., nk],
representa as covariâncias cruzadas entre as estimativas das poses indicativas das regiões do ambiente.
Então, aumenta-se a matriz de covariâncias P−
k com a incerteza associada à pose atual
do robô, Prr k Pk+ = Prr k Pkr1 · · · P rnk k 0 Pr1 k Pk11 · · · P 1nk k 0 ... ... . .. ... ... Prnk k P 1nk k · · · P nknk k 0 0 0 · · · 0 Prr k (3.47)
A matriz de covariâncias final é calculada então da seguinte forma:
Pk+ = Prr k Pkr1 · · · P rnk k Pkrr Pr1 k Pk11 · · · P 1nk k Pkr1 ... ... . .. ... ... Prnk k P 1nk k · · · P nknk k P rnk k Prr k Pkr1 · · · P rnk k Pkrr (3.48)
3.6 UMA NOVA VERSÃO PARA O FILTRO IMM APLICADO AO PROBLEMA