Kalman Filter

Existem muitos artigos diferentes sobre o filtro Kalman, mas é difícil encontrar o que contém uma explicação, de onde todas as fórmulas de filtragem vêm. Eu acho que, sem entender isso, essa ciência se torna completamente incompreensível. Neste artigo, tentarei explicar tudo de uma maneira simples.

O filtro Kalman é uma ferramenta muito poderosa para filtrar diferentes tipos de dados. A principal idéia por trás disso de que se deve usar uma informação sobre o processo físico. Por exemplo, se você estiver filtrando dados do velocímetro de um carro, sua inércia lhe dará o direito de tratar um grande desvio de velocidade como um erro de medição. O filtro Kalman também é interessante pelo fato de que, de alguma forma, é o melhor filtro. Vamos discutir exatamente o que isso significa. No final do artigo, mostrarei como é possível simplificar as fórmulas.

Preliminares

Primeiramente, vamos memorizar algumas definições e fatos da teoria das probabilidades.

Variável aleatória

Quando se diz que é dada uma variável aleatória $$$\ xi$ , significa que pode levar valores aleatórios. Valores diferentes vêm com probabilidades diferentes. Por exemplo, se alguém joga um dado, o conjunto de valores é discreto $$$\ {1,2,3,4,5,6 \}$ . Quando você lida com uma velocidade de movimentação de partículas, obviamente você deve trabalhar com um conjunto contínuo de valores. Valores que saem após cada experimento (medida) que indicaríamos por $$$x_1, x_2, ...$ , mas às vezes usamos a mesma letra que usamos para uma variável aleatória $$$\ xi$ . No caso de um conjunto contínuo de valores, uma variável aleatória é caracterizada por sua função de densidade de probabilidade $$$\ rho (x)$ . Esta função mostra uma probabilidade de que a variável aleatória caia em uma pequena vizinhança $$$dx$ do ponto $$$x$ . Como podemos ver na figura, essa probabilidade é igual à área do retângulo tracejado abaixo do gráfico $$$\ rho (x) dx$ .

Muitas vezes, em nossa vida, variáveis ​​aleatórias têm a distribuição de Gauss, quando a densidade de probabilidade é $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ .

Podemos ver que a função em forma de sino $$$\ rho (x)$ está centrado no ponto $$$\ mu$ e sua largura característica é de cerca de $$$\ sigma$ .
Já que estamos falando sobre a distribuição gaussiana, seria um pecado não mencionar de onde ela vem. Bem como o número $$$e$ e $$$\ pi$ são firmemente penetrados na matemática e podem ser encontrados nos lugares mais inesperados; portanto, a distribuição gaussiana tem raízes profundas na teoria da probabilidade. A seguinte declaração notável explica em parte a presença da distribuição de Gauss em muitos processos:
Deixe uma variável aleatória $$$\ xi$ tem uma distribuição arbitrária (de fato, existem algumas restrições à arbitrariedade, mas elas não são restritivas). Vamos realizar $$$n$ experimentos e calcular uma soma $$$\ xi_1 + ... + \ xi_n$ , de valores caídos. Vamos fazer muitas experiências. É claro que toda vez obteremos um valor diferente da soma. Em outras palavras, essa soma é uma variável aleatória com sua própria lei de distribuição. Acontece que, para empresas suficientemente grandes $$$n$ , a lei de distribuição dessa soma tende a uma distribuição gaussiana (a propósito, a largura característica de um sino está crescendo como $$$\ sqrt n$ . Leia mais na Wikipedia: Teorema do limite central . Na vida real, existem muitos valores que são uma soma de um grande número de variáveis ​​aleatórias independentes e identicamente distribuídas. Portanto, esses valores têm distribuição Gauss.

Valor de significado

Por definição, um valor médio de uma variável aleatória é um valor que chegamos a um limite se realizarmos mais e mais experimentos e calcularmos uma média de valores caídos. Um valor médio é denotado de diferentes maneiras: os matemáticos denotam por $$$E \ xi$ (expectativa), os físicos o denotam por $$$\ overline {\ xi}$ ou $$$<\ xi>$ . Vamos denotá-lo como matemáticos.

Por exemplo, um valor médio de distribuição gaussiana $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ é igual a $$$\ mu$ .

Variação

Para a distribuição gaussiana, vemos claramente que a variável aleatória tende a cair dentro de uma determinada região de seu valor médio $$$\ mu$ . Vamos apreciar a distribuição gaussiana mais uma vez:

Na figura, pode-se ver que uma largura característica de uma região onde os valores caem principalmente é $$$\ sigma$ . Como podemos estimar essa largura para uma variável aleatória arbitrária? Podemos desenhar um gráfico de sua função de densidade de probabilidade e apenas avaliar visualmente a faixa de características. No entanto, seria melhor escolher uma maneira algébrica precisa para esta avaliação. Podemos encontrar um comprimento médio de desvio do valor médio: $$$E | \ xi-E \ xi |$ . Esse valor é uma boa estimativa de um desvio característico de $$$\ xi$ . No entanto, sabemos muito bem como é problemático usar valores absolutos em fórmulas, portanto, essa fórmula raramente é usada na prática.

Uma abordagem mais simples (simples do ponto de vista do cálculo) é calcular $$$E (\ xi-E \ xi) ^ 2$ .

Esse valor chamado variação e denotado por $$$\ sigma_ \ xi ^ 2$ . A raiz quadrática da variância é uma boa estimativa do desvio característico da variável aleatória. É chamado de desvio padrão.

Por exemplo, pode-se calcular isso para a distribuição gaussiana $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ a variância é igual a $$$\ sigma ^ 2$ assim, o desvio padrão é $$$\ sigma$ . Esse resultado realmente corresponde à nossa intuição geométrica. De fato, uma pequena trapaça está escondida aqui. Na verdade, em uma definição da distribuição Gauss, você vê o número $$$2$ em um denominador de expressão $$$- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}$ . Isto $$$2$ fica ali de propósito, para o desvio padrão $$$\ sigma_ \ xi$ ser igual exatamente a $$$\ sigma$ . Portanto, a fórmula da distribuição de Gauss é escrita de uma maneira, que tenha em mente que alguém calcularia seu desvio padrão.

Variáveis ​​aleatórias independentes

Variáveis ​​aleatórias podem depender umas das outras ou não. Imagine que você está jogando uma agulha no chão e medindo as coordenadas das duas extremidades. Essas duas coordenadas são variáveis ​​aleatórias, mas dependem uma da outra, pois a distância entre elas deve ser sempre igual ao comprimento da agulha. Variáveis ​​aleatórias são independentes uma da outra se os resultados decrescentes da primeira não dependerem dos resultados da segunda. Para duas variáveis ​​independentes $$$\ xi_1$ e $$$\ xi_2$ a média do seu produto é igual ao produto da sua média: $$$E (\ xi_1 \ cdot \ xi_2) = E \ xi_1 \ cdot \ xi_2$

Filtro de Kalman

Declaração do Problema

Vamos denotar por $$$x_k$ um valor que pretendemos medir e depois filtrar. Pode ser uma coordenada, velocidade, aceleração, umidade, temperatura, pressão, etc.

Vamos começar com um exemplo simples, que nos levará à formulação do problema geral. Imagine que você tem um carro de brinquedo com controle de rádio que pode correr apenas para frente e para trás. Conhecendo sua massa, forma e outros parâmetros do sistema, calculamos como a maneira como um joystick de controle age na velocidade de um carro $$$v_k$ .


A coordenada do carro seria pela seguinte fórmula

$$

$$x_ {k + 1} = x_k + v_kdt$$

Na vida real, não podemos, não podemos ter uma fórmula precisa para as coordenadas, já que alguns pequenos distúrbios agindo no carro como vento, solavancos, pedras na estrada, portanto a velocidade real do carro será diferente da calculada . Então adicionamos uma variável aleatória $$$\ xi_k$ para o lado direito da última equação:

$$

$$x_ {k + 1} = x_k + v_kdt + \ xi_k$$

Também temos um sensor GPS no carro que tenta medir a coordenada do carro $$$x_k$ . Claro que há um erro nessa medição, que é uma variável aleatória $$$\ eta_k$ . Portanto, a partir do sensor, obteríamos dados errados:

$$

$$z_k = x_k + \ eta_k$$

Nosso objetivo é encontrar uma boa estimativa para a verdadeira coordenada $$$x_k$ , conhecendo os dados de um sensor errado $$$z_k$ . Essa boa estimativa, denotaremos por $$$x ^ {opt}$ .
Em geral, a coordenada $$$x_k$ pode representar qualquer valor (temperatura, umidade, ...) e o membro de controle que indicaríamos por $$$u_k$ (no exemplo com um carro $$$u_k = v_k \ cdot dt$ ) As equações para as coordenadas e as medições do sensor seriam as seguintes:

(1)

Vamos discutir o que sabemos nessas equações.

• $$$u_k$ é um valor conhecido que controla uma evolução do sistema. Nós sabemos disso pelo modelo do sistema.
• A variável aleatória $$$\ xi$ representa o erro no modelo do sistema e a variável aleatória $$$\ eta$ é um erro do sensor. Suas leis de distribuição não dependem do tempo (no índice de iteração $$$k$ )
• Os meios de erros são iguais a zero: $$$E \ eta_k = E \ xi_k = 0$ .
• Podemos não conhecer uma lei de distribuição das variáveis ​​aleatórias, mas sabemos suas variações $$$\ sigma_ \ xi$ e $$$\ sigma_ \ eta$ . Observe que as variações não dependem do tempo (em $$$k$ ), uma vez que as leis de distribuição correspondentes também não.
• Supomos que todos os erros aleatórios sejam independentes um do outro: o erro no momento $$$k$ não depende do erro no momento $$$k '$ .

Observe que um problema de filtragem não é um problema de suavização. Nosso objetivo não é suavizar os dados de um sensor, apenas queremos obter o valor o mais próximo possível da coordenada real $$$x_k$ .

Algoritmo de Kalman

Nós usaríamos uma indução. Imagine que na etapa $$$k$ já encontramos o valor do sensor filtrado $$$x ^ {opt}$ , que é uma boa estimativa da coordenada real $$$x_k$ . Lembre-se de que conhecemos a equação que controla a coordenada real:

$$

$$x_ {k + 1} = x_k + u_k + \ xi_k$$

Portanto, antes de obter o valor do sensor, podemos afirmar que ele mostraria o valor próximo a $$$x ^ {opt} + u_k$ . Infelizmente até agora não podemos dizer algo mais preciso. Mas na etapa $$$k + 1$ teríamos uma leitura não precisa do sensor $$$z_ {k + 1}$ .
A ideia de Kalman é a seguinte. Para obter a melhor estimativa da coordenada real $$$x_ {k + 1}$ devemos obter um meio de ouro entre a leitura do sensor não preciso $$$z_ {k + 1}$ e $$$x ^ {opt} + u_k$ - nossa previsão, o que esperamos ver no sensor. Vamos dar um peso $$$K$ ao valor do sensor e $$$(1-K)$ para o valor previsto:

$$

$$x ^ {opt} _ {k + 1} = K \ cdot z_ {k + 1} + (1-K) \ cdot (x_k ^ {opt} + u_k)$$

O coeficiente $$$K$ é chamado de coeficiente de Kalman. Depende do índice de iteração e, estritamente falando, devemos escrever $$$K_ {k + 1}$ . Mas, para manter as fórmulas em uma forma agradável, omitiríamos o índice de $$$K$ .
Devemos escolher o coeficiente de Kalman de maneira que a coordenada estimada $$$x ^ {opt} _ {k + 1}$ seria o mais próximo possível da coordenada real $$$x_ {k + 1}$ .
Por exemplo, se soubermos que nosso sensor é muito preciso, confiaremos na leitura e daremos a ele um grande peso ( $$$K$ está perto de um). Se o sensor, por outro lado, não for preciso, confiaremos em nosso valor previsto teoricamente $$$x ^ {opt} _k + u_k$ .
Em geral, devemos minimizar o erro de nossa estimativa:

$$

$$e_ {k + 1} = x_ {k + 1} -x ^ {opt} _ {k + 1}$$

Usamos as equações (1) (aquelas que estão em uma moldura azul), para reescrever a equação para o erro:

$$

$$e_ {k + 1} = (1-K) (e_k + \ xi_k) - K \ eta_ {k + 1}$$

Agora chega a hora de discutir, o que significa a expressão “minimizar o erro”? Sabemos que o erro é uma variável aleatória e, portanto, cada vez que utiliza valores diferentes. Na verdade, não há uma resposta única para essa pergunta. Da mesma forma, foi no caso da variância de uma variável aleatória, quando estávamos tentando estimar a largura característica de sua função de densidade de probabilidade. Então, escolheríamos um critério simples. Minimizaríamos uma média do quadrado:

$$

$$E (e ^ 2_ {k + 1}) \ rightarrow min$$

Vamos reescrever a última expressão:

$$

$$E (e ^ 2_ {k + 1}) = (1-K) ^ 2 (E_k ^ 2 + \ sigma ^ 2_ \ xi) + K ^ 2 \ sigma ^ 2_ \ eta$$

A última expressão assume seu valor mínimo, quando sua derivação é zero. Então quando:

$$

$$\ displaystyle K_ {k + 1} = \ frac {Ee ^ 2_k + \ sigma ^ 2_ \ xi} {Ee ^ 2_k + \ sigma ^ 2_ \ xi + \ sigma ^ 2_ \ eta}$$

Aqui, escrevemos o coeficiente de Kalman com seu subscrito; portanto, enfatizamos o fato de que isso depende da etapa da iteração. Substituímos a equação pelo erro quadrático médio $$$E (e ^ 2_ {k + 1})$ o coeficiente de Kalman $$$K_ {k + 1}$ que minimizam seu valor:

$$

$$\ displaystyle E (e ^ 2_ {k + 1}) = \ frac {\ sigma ^ 2_ \ eta (Ee ^ 2_k + \ sigma ^ 2_ \ xi)} {Ee ^ 2_k + \ sigma ^ 2_ \ xi + \ sigma ^ 2_ \ eta}$$

Então, resolvemos o nosso problema. Temos a fórmula iterativa para calcular o coeficiente de Kalman.

Exemplo

Na trama do início deste artigo, existem dados filtrados do sensor GPS fictício, instalados no carro fictício, que se move com a aceleração constante $$$a$ .

$$

$$x_ {t + 1} = x_t + em \ cdot dt + \ xi_t$$

Análise

Se olharmos como o coeficiente de Kalman $$$K_k$ mudanças da iteração $$$k$ , é possível ver que ele se estabiliza no valor certo $$$K_ {stab}$ . Por exemplo, se os erros quadráticos médios do sensor e o modelo se respeitarem como dez para um, o gráfico da dependência do coeficiente de Kalman da etapa de iteração seria o seguinte:


No próximo exemplo, discutiremos como isso pode simplificar nossa vida.

Segundo exemplo

Na prática, acontece que não sabemos quase nada do modelo físico sobre o que estamos filtrando. Imagine que você decidiu filtrar essas medidas do seu acelerômetro favorito. Na verdade, você não sabe como o acelerômetro seria movido. A única coisa que você deve saber é a variação do erro do sensor $$$\ sigma ^ 2_ \ eta$ . Nesse problema difícil, podemos colocar todas as informações desconhecidas do modelo físico na variável aleatória $$$\ xi_k$ :

$$

$$x_ {k + 1} = x_k + \ xi_k$$

Estritamente falando, esse tipo de sistema não satisfaz a condição que impusemos à variável aleatória $$$\ xi_k$ . Uma vez que contém as informações desconhecidas para nós, a física do movimento. Não podemos dizer que em momentos diferentes os erros são independentes um do outro e suas médias são iguais a zero. Em outras palavras, significa que para esse tipo de situação a teoria de Kalman não é aplicada. Mas de qualquer maneira, podemos tentar usar o mecanismo da teoria de Kalman apenas escolhendo alguns valores razoáveis ​​para $$$\ sigma_ \ xi ^ 2$ e $$$\ sigma_ \ eta ^ 2$ para obter apenas um bom gráfico de dados filtrados.
Mas há uma maneira muito mais simples. Vimos que com o aumento da etapa $$$k$ o coeficiente de Kalman sempre se estabiliza para um determinado valor $$$K_ {stab}$ . Então, ao invés de adivinhar os valores dos coeficientes $$$\ sigma ^ 2_ \ xi$ e $$$\ sigma ^ 2_ \ eta$ e computando o coeficiente de Kalman $$$K_k$ por fórmulas difíceis, podemos assumir que esse coeficiente é constante e selecionar apenas essa constante. Essa suposição não afetaria muito os resultados da filtragem. De início, de qualquer maneira, o mecanismo de Kalman não é exatamente aplicável ao nosso problema e, em segundo lugar, o coeficiente de Kalman se estabiliza rapidamente na constante. No final, tudo se torna muito simples. Não precisamos de quase nenhuma fórmula da teoria de Kalman, apenas precisamos selecionar um valor razoável $$$K_ {stab}$ e insira-o na fórmula iterativa

$$

$$x ^ {opt} _ {k + 1} = K_ {facada} \ cdot z_ {k + 1} + (1-K_ {facada}) \ cdot x_k ^ {opt}$$

No próximo gráfico, você pode ver as medições filtradas por duas maneiras diferentes de um sensor imaginário. O primeiro método é o honesto, com todas as fórmulas da teoria de Kalman. O segundo método é o simplificado.


Vemos que não há uma grande diferença entre dois desses métodos. Há uma pequena variação no início, quando o coeficiente de Kalman ainda não está estabilizado.

Discussão

Vimos que a idéia principal do filtro Kalman é escolher o coeficiente $$$K$ de uma maneira que o valor filtrado

$$

$$x ^ {opt} _ {k + 1} = Kz_ {k + 1} + (1-K) (x ^ {opt} _k + u_k)$$

em média, seria o mais próximo possível da coordenada real $$$x_ {k + 1}$ . Vemos que o valor filtrado $$$x ^ {opt} _ {k + 1}$ é uma função linear da medição do sensor $$$z_ {k + 1}$ e o valor filtrado anterior $$$x ^ {opt} _k$ . Mas o valor filtrado anterior $$$x ^ {opt} _k$ em si é uma função linear da medição do sensor $$$z_k$ e o valor filtrado pré-anterior $$$x ^ {opt} _ {k-1}$ . E assim por diante até o fim da cadeia. Portanto, o valor filtrado depende linearmente de todas as leituras do sensor anterior:

$$

$$x ^ {opt} _ {k + 1} = \ lambda + \ lambda_0z_0 + \ ldots + \ lambda_ {k + 1} z_ {k + 1}$$

Essa é a razão pela qual o filtro Kalman é chamado de filtro linear. É possível provar que o filtro Kalman é o melhor de todos os filtros lineares. O melhor no sentido de minimizar a média quadrada do erro.

Caso multidimensional

É possível generalizar toda a teoria de Kalman para o caso multidimensional. As fórmulas parecem um pouco mais elaboradas, mas a idéia de derivar ainda permanece a mesma de uma dimensão. Por exemplo, neste belo vídeo, você pode ver o exemplo.

Literatura

O artigo original, escrito por Kalman, pode ser baixado aqui:
http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf