Tutorial de Filtro de Kalman
(kalmanfilter.net)- Filtro de Kalman é um algoritmo que usa em conjunto medições de sensores com ruído e modelos dinâmicos imperfeitos para estimar o estado atual e o próximo estado, incluindo a incerteza de ambos
- O tutorial acompanha numericamente, com um exemplo de rastreamento por radar de aeronave, o processo de combinar valores previstos e medidos usando a distância (r) e a velocidade (v) como vetor de estado
- Usando a medição inicial (10,000m), (200m/s) e o intervalo de amostragem (5s), a próxima posição é prevista como (11,000m) em um modelo de velocidade constante, e o ruído de medição (R) e o ruído de processo (Q) são refletidos na covariância
- A segunda medição, (11,020m) e (202m/s), é mais incerta, mas o ganho de Kalman (K) combina previsão e medição com pesos e calcula o estado atualizado como (11,009.37m) e (201.43m/s)
- Após a inicialização, o loop de previsão-atualização se repete, e em implementações reais também é preciso considerar fórmulas estáveis de atualização da covariância, como a forma de Joseph, além do tratamento de medições anômalas
O problema de estimação que o filtro de Kalman resolve
- O Filtro de Kalman é um algoritmo para estimar e prever o estado de um sistema em um ambiente com incerteza
- Dados de sensores com ruído de medição
- Fatores externos desconhecidos
- Diferenças entre o modelo dinâmico e o movimento real
- É usado em rastreamento de objetos, navegação, robótica, controle, análise de mercados financeiros e previsão do tempo
- Quando aplicado à estimativa da trajetória do mouse do computador, pode reduzir ruído e compensar tremores da mão para produzir um caminho de movimento mais estável
- O tutorial foi estruturado para ajudar a entender o Filtro de Kalman com exemplos numéricos e explicações intuitivas, mais do que com descrições matemáticas complexas
- Também inclui exemplos em que um Filtro de Kalman mal projetado não consegue rastrear corretamente um objeto e como corrigir isso
Trilha de aprendizado
- Este projeto foi estruturado para permitir aprender o filtro de Kalman em três níveis de profundidade
- Visão geral em página única: explica a ideia central e as equações essenciais sem derivação, assumindo conhecimento básico de estatística e álgebra linear
- Tutorial web gratuito baseado em exemplos: desenvolve a intuição com exemplos numéricos e aborda passo a passo até a derivação das equações do filtro de Kalman, informando que não exige conhecimento prévio
- Kalman Filter from the Ground Up: inclui 14 exemplos numéricos completos e resolvidos, gráficos de desempenho e tabelas, Extended Kalman Filter, Unscented Kalman Filter, fusão de sensores e diretrizes de implementação
A necessidade de previsão em um exemplo de rastreamento por radar
- Em um radar que rastreia uma aeronave, a aeronave é o sistema, e a posição a ser estimada é o estado do sistema
- Como o radar aponta um feixe estreito na direção da aeronave, é preciso prever a posição futura para decidir para onde enviar o próximo feixe
- Se a previsão falhar, o feixe pode apontar para a direção errada e o rastreamento pode ser perdido
- É necessário um modelo dinâmico que represente o movimento do sistema ao longo do tempo
- No exemplo simplificado unidimensional, assume-se que a aeronave se move em linha reta na direção do radar ou se afastando dele
- O radar calcula a distância (r) pelo tempo de transmissão e recepção do pulso
- A velocidade (v) também pode ser medida pelo efeito Doppler
- Em (t_0), se a distância (10,000m) e a velocidade (200m/s) forem medidas com muita precisão, com intervalo de amostragem (\Delta t=5s), e assumindo velocidade constante, a próxima posição será (11,000m)
- (\Delta r = v \cdot \Delta t)
- (r_{t_1}=10,000+200\cdot5=11,000m)
Ruído de medição e ruído de processo
- Medições reais de radar não são perfeitamente precisas, então mesmo que vários radares meçam no mesmo instante, eles podem produzir valores ligeiramente diferentes
- Essa variação é representada como ruído de medição
- É preciso calcular não só a estimativa do estado, mas também o quanto essa estimativa é confiável
- O modelo dinâmico também não é perfeito
- Mesmo assumindo que a aeronave se move com velocidade constante, o movimento real pode ser diferente por fatores externos imprevisíveis, como o vento
- Esse tipo de influência imprevisível é o ruído de processo
- O filtro de Kalman fornece em conjunto a estimativa do estado atual, a previsão do estado futuro e a incerteza de cada um
- É um algoritmo ótimo que minimiza a incerteza da estimativa do estado sob a condição de que o sistema e o ruído sigam as hipóteses do modelo
Vetor de estado e inicialização
- O estado do sistema no exemplo é composto pela distância (r) e pela velocidade (v) da aeronave
[ \boldsymbol{x}= \begin{bmatrix} r\ v \end{bmatrix} ]
- A primeira medição em (t_0) é a seguinte
[ \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- Como as medições têm incerteza, cada medição vem acompanhada de uma incerteza de medição na forma de variância
- Desvio padrão da medição de distância: (4m)
- Desvio padrão da medição de velocidade: (0.5m/s)
- A variância é o quadrado do desvio padrão
[ \boldsymbol{R}_0= \begin{bmatrix} 16 & 0\ 0 & 0.25 \end{bmatrix} ]
- Neste exemplo, assume-se que os erros de medição de distância e velocidade não estão correlacionados entre si, então os elementos fora da diagonal da matriz de covariância são definidos como 0
- Na etapa de inicialização, como o valor medido e o estado do sistema representam as mesmas grandezas físicas (r) e (v), a primeira medição pode ser usada como estimativa inicial do estado
[ \hat{\boldsymbol{x}}_{0,0}= \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- Essa abordagem só pode ser usada na etapa de inicialização
Etapa de previsão: propagação do estado e da covariância
- A previsão calcula o estado no instante seguinte usando o estado atual e a matriz de transição de estado (\boldsymbol{F})
- No modelo de velocidade constante, usa-se a seguinte equação
[ v_1=v_0=v ]
[ r_1=r_0+v_0\Delta t ]
- A equação de previsão do estado em forma matricial é a seguinte
[ \hat{\boldsymbol{x}}_{n+1,n}
\boldsymbol{F} \hat{\boldsymbol{x}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n ]
- (\boldsymbol{u}_n): variável de entrada
- (\boldsymbol{G}): matriz de transição da entrada
- Neste exemplo simples, não há entrada, então (\boldsymbol{u}_n=0)
- Quando (\Delta t=5s), a matriz de transição de estado é a seguinte, e o resultado previsto é (11,000m) e (200m/s)
[ \boldsymbol{F}= \begin{bmatrix} 1 & 5\ 0 & 1 \end{bmatrix} ]
[ \hat{\boldsymbol{x}}_{1,0}
\begin{bmatrix} 11{,}000\ 200 \end{bmatrix} ]
- A previsão da covariância não usa simplesmente (\boldsymbol{F}\boldsymbol{P}), mas sim (\boldsymbol{F}\boldsymbol{P}\boldsymbol{F}^T)
[ \boldsymbol{P}_{n+1,n}
\boldsymbol{F} \boldsymbol{P}_{n,n} \boldsymbol{F}^T + \boldsymbol{Q} ]
- Excluindo o ruído de processo, a covariância prevista é a seguinte
[ \boldsymbol{P}_{1,0}
\begin{bmatrix} 22.25 & 1.25\ 1.25 & 0.25 \end{bmatrix} ]
- A variância da velocidade permanece em (0.25) por causa do modelo de velocidade constante
- A variância da distância aumenta de (16) para (22.25), porque a incerteza da velocidade aumenta a incerteza da distância ao longo do tempo
Incorporando o ruído de processo
- Como a velocidade real da aeronave pode ser afetada por fatores externos imprevisíveis, como o vento, o ruído de processo (\boldsymbol{Q}) é somado à previsão da covariância
- No exemplo, assume-se um desvio padrão de aceleração aleatória de (\sigma_a=0.2m/s^2)
- A variância é (\sigma_a^2=0.04m^2/s^4)
- Quando (\Delta t=5s), a matriz de ruído de processo é a seguinte
[ \boldsymbol{Q}
\begin{bmatrix} 6.25 & 2.5\ 2.5 & 1 \end{bmatrix} ]
- A covariância prevista com o ruído de processo adicionado é a seguinte
[ \boldsymbol{P}_{1,0}
\begin{bmatrix} 28.5 & 3.75\ 3.75 & 1.25 \end{bmatrix} ]
Etapa de atualização: combinação ponderada entre previsão e medição
- Em (t_1), a segunda medição é a seguinte
[ \boldsymbol{z}_1= \begin{bmatrix} 11{,}020\ 202 \end{bmatrix} ]
- Assume-se que essa medição tem maior incerteza do que a primeira, porque um pico forte de ruído reduziu a relação sinal-ruído
- Desvio padrão da distância: (6m)
- Desvio padrão da velocidade: (1.5m/s)
[ \boldsymbol{R}_1= \begin{bmatrix} 36 & 0\ 0 & 2.25 \end{bmatrix} ]
- Como os elementos diagonais da covariância prevista (\boldsymbol{P}_{1,0}) são menores que os da covariância de medição (\boldsymbol{R}_1), a incerteza do lado da previsão é menor
- O filtro de Kalman não usa só a previsão nem só a medição: ele combina as duas dando peso maior ao lado com menor incerteza
- A média ponderada na forma unidimensional é a seguinte
[ \hat{x}_{1,1}
K_1 z_1 + (1-K_1)\hat{x}_{1,0} ]
- (\boldsymbol{K}) é o ganho de Kalman e determina os pesos da medição e da previsão de modo a minimizar a incerteza da estimativa atualizada
Inovação, matriz de observação e ganho de Kalman
- A equação de atualização do estado pode ser escrita como a previsão mais um termo de correção
[ \hat{\boldsymbol{x}}_{1,1}
\hat{\boldsymbol{x}}_{1,0} + \boldsymbol{K}_1 ( \boldsymbol{z}_1
\boldsymbol{H}\hat{\boldsymbol{x}}_{1,0} ) ]
- (\boldsymbol{z}1-\boldsymbol{H}\hat{\boldsymbol{x}}{1,0}) é a inovação (innovation) ou resíduo (residual), representando a informação fornecida pela nova medição
- (\boldsymbol{H}) é a matriz de observação ou matriz de medição, que mapeia as variáveis de estado para as grandezas físicas efetivamente medidas
- Neste exemplo, como estado e medição são ambos distância e velocidade, (\boldsymbol{H}=\boldsymbol{I})
- Em geral, os valores medidos e o estado podem estar em domínios físicos diferentes, como em um termômetro digital
- O ganho de Kalman multivariado é o seguinte
[ \boldsymbol{K}_n
\boldsymbol{P}{n,n-1} \boldsymbol{H}^T ( \boldsymbol{H} \boldsymbol{P}{n,n-1} \boldsymbol{H}^T + \boldsymbol{R}_n )^{-1} ]
- O ganho de Kalman calculado no exemplo é o seguinte
[ \boldsymbol{K}_1= \begin{bmatrix} 0.4048 & 0.6377\ 0.0399 & 0.3144 \end{bmatrix} ]
- O cálculo da inversa de matriz pode ser feito com
inv(A)no MATLAB ounumpy.linalg.inv(A)no Python, mas em implementações reais geralmente é melhor resolver diretamente o sistema linear comA\bounumpy.linalg.solve(A, b)do que usar a inversa explícita
Resultado da atualização e redução da covariância
- A inovação neste exemplo é a seguinte
[ \boldsymbol{z}1-\hat{\boldsymbol{x}}{1,0}
\begin{bmatrix} 20\ 2 \end{bmatrix} ]
- Calculando o termo de correção com o ganho de Kalman, obtém-se o seguinte
[ \boldsymbol{K}_1 \begin{bmatrix} 20\ 2 \end{bmatrix}
\begin{bmatrix} 9.37\ 1.43 \end{bmatrix} ]
- A estimativa de estado atualizada é a seguinte
[ \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 11{,}009.37\ 201.43 \end{bmatrix} ]
- Na atualização de covariância multivariada, costuma-se usar a forma de Joseph, que é numericamente estável
[ \boldsymbol{P}_{n,n}
(\boldsymbol{I}-\boldsymbol{K}n\boldsymbol{H}) \boldsymbol{P}{n,n-1} (\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H})^T + \boldsymbol{K}_n \boldsymbol{R}_n \boldsymbol{K}_n^T ]
- A forma simplificada de atualização da covariância também aparece com frequência na literatura
[ \boldsymbol{P}_{n,n}
(\boldsymbol{I}-\boldsymbol{K}n\boldsymbol{H}) \boldsymbol{P}{n,n-1} ]
- Em aritmética exata, as duas formas produzem o mesmo resultado, mas em implementações computacionais a forma de Joseph geralmente é mais estável numericamente
- No exemplo, a covariância atualizada calculada com a fórmula simplificada é a seguinte
[ \boldsymbol{P}_{1,1}
\begin{bmatrix} 14.57 & 1.43\ 1.43 & 0.71 \end{bmatrix} ]
- Os elementos diagonais da covariância atualizada são menores do que os da covariância prevista ((28.5, 1.25)) e os da covariância de medição ((36, 2.25))
- Mesmo quando a nova informação tem alta incerteza, ela reduz a incerteza da estimativa, e teoricamente uma nova medição não deve ser ignorada
- Em implementações reais, pode haver casos em que medições não confiáveis precisam ser rejeitadas, e métodos de tratamento de outliers são abordados no capítulo Outlier Treatment do livro
Próxima previsão e loop repetitivo
- A etapa de previsão da Iteration 1 é igual à da Iteration 0, mas o ponto de partida passa a ser o (\hat{\boldsymbol{x}}{1,1}) e a (\boldsymbol{P}{1,1}) atualizados
- O resultado da previsão de estado é o seguinte
[ \hat{\boldsymbol{x}}_{2,1}
\boldsymbol{F} \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 12{,}016.5\ 201.43 \end{bmatrix} ]
- O resultado da previsão da covariância é o seguinte
[ \boldsymbol{P}_{2,1}
\begin{bmatrix} 52.86 & 7.47\ 7.47 & 1.71 \end{bmatrix} ]
- Se o tempo passar sem novas medições, a incerteza aumenta naturalmente, então a variância volta a crescer na etapa de previsão
- A incerteza da velocidade aumenta ainda mais a incerteza da distância
- Por isso a variância da distância cresce mais rápido do que a da velocidade
- O exemplo mostra as três etapas do filtro de Kalman
- Inicialização: executada uma vez no início
- Previsão: propaga o próximo estado e sua incerteza com o modelo dinâmico
- Atualização: combina a nova medição e a previsão com o ganho de Kalman
- Depois da inicialização, o filtro de Kalman continua operando em um loop de previsão-atualização
1 comentários
Comentários do Hacker News
Sempre acabo dizendo que aprender filtro de Kalman isoladamente inverte a ordem das coisas, então é fácil perder os grandes insights que a teoria ao redor abre
Para entender direito, é melhor passar por mínimos quadrados (regressão linear), mínimos quadrados recursivos e filtro de informação (outra formulação do KF), nessa ordem
Aí dá para perceber que o KF é apenas mínimos quadrados recursivos reformulados para priorizar a eficiência da etapa de atualização
Este PDF dá uma visão geral concisa: http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/...
Ainda assim, existe curiosidade intelectual, e é preciso haver um caminho para ir avançando aos poucos rumo ao entendimento sem perder essa curiosidade
Mesmo relendo The Six (Not So) Easy Pieces sem entender, ainda vale a pena, e brincar com o gato de Arnold permite experimentar, com uma curiosidade de primata meio crua e sem um procedimento científico rigoroso, conceitos que originalmente estavam atrás da porta chamada contexto
http://gerdbreitenbach.de/arnold_cat/cat.html
Em 1 dimensão, na previsão linear X'1 = X0*a + b, obtém-se a distribuição a priori, com mean(X'1) = mean(X0)*a + b e var(X'1) = var(X0)*a^2, em que a e b representam a dinâmica assumida
A posteriori gaussiana é uma média ponderada pela precisão entre a priori e a observação, então X1 = (1 - K)X'1 + YK, e K = (1/var(X'1))/(1/var(X'1) + 1/var(Y)), em que Y é uma observação gaussiana
Repetindo isso, você obtém o filtro de Kalman, e se conhecer a linearidade das gaussianas multidimensionais, também fica intuitivo generalizar para múltiplas dimensões
Só que a linearidade das gaussianas multidimensionais e a própria posteriori gaussiana talvez não sejam assuntos tão simples assim
Sempre que esse assunto aparece, este material aparece junto, e o contrário também acontece: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
O uso de Jupyter notebooks também é excelente
Parece que ainda não existe uma ferramenta de cálculo simbólico para distribuições de probabilidade
Por exemplo, uma ferramenta que multiplique duas funções densidade de probabilidade gaussianas multivariadas para obter a matriz de covariância, ou em que você defina todos os componentes do filtro de Kalman (modelo de previsão e processo de observação) e ela gere as fórmulas necessárias como o
lambdifydo sympyNão sei, porém, se o Sympy consegue tratar também as distribuições condicionais de que o filtro de Kalman precisa, isto é, a posteriori bayesiana
De todo modo, se você quiser brincar com filtro de Kalman no Sympy, é melhor lidar diretamente com média e variância, ou com a matriz de covariância
Referência: https://reference.wolfram.com/language/howto/WorkWithStatist...
E também: https://reference.wolfram.com/language/ref/MultinormalDistri...
https://www.squiggle-language.com/docs
Se Q e R forem constantes, como costuma acontecer, o ganho converge rapidamente, então o filtro de Kalman fica praticamente igual a um filtro exponencial com etapa de previsão
Para muita gente, essa explicação é bem mais fácil de entender e combina melhor com a forma como ele é usado na prática
Normalmente, Q e R são ajustados manualmente até “parecer bom”, e depois não se mexe mais neles
Além disso, em vez de ajustar vários valores como Q e R, você pode ajustar manualmente apenas um ganho
A ideia é simplesmente ir ajustando até o resultado parecer plausível? Nesse caso, não entendo como isso funciona direito mesmo quando a situação não está completamente sobreajustada
Por exemplo, se você estiver rastreando um pássaro em vídeo, até dá para escolher algum Q, mas as estatísticas de ruído podem mudar ao longo do tempo. O que se faz nesse caso?
Post relacionado: Kalman filter from the ground up - https://news.ycombinator.com/item?id=37879715 - outubro de 2023, 150 comentários
Também fico curioso sobre qual seria o melhor ano para colocar no título acima
O filtro de Kalman aparece em um tema mais geral no livro Optimization by Vector Space Methods, de David G. Luenberger, John Wiley and Sons, Inc., New York, 1969
Me ocorreu agora esta ideia. Será que um caso com apenas depoimentos de testemunhas poderia ser codificado de algum modo em vetores e então tratado com um filtro de Kalman para reforçar o valor probatório das observações?
Seria uma forma de tratar tanto mentira quanto imprecisão como “erro”
Estou pensando nas luzes de Phoenix, ou em UFOs de modo geral, fantasmas, experiências de quase morte e, de forma mais cotidiana, alegações de estupro
Quase sempre o melhor material é este aqui: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
Mesmo para quem não usa Python, é excelente e cobre muito bem o panorama geral
Mais alguém aqui assistiu às aulas sobre filtro de Kalman do Michael van Biezem de gravata-borboleta ao aprender esse assunto?
https://www.youtube.com/watch?v=CaCcOwJPytQ&list=PLX2gX-ftPV...
A única frase que você realmente precisa saber é esta: “Este filtro recebeu o nome de Rudolf E. Kálmán (19 de maio de 1930 – 2 de julho de 2016). Em 1960, Kálmán publicou seu famoso artigo descrevendo uma solução recursiva para o problema de filtragem linear de dados discretos”