Este capítulo descreve o Filtro de Kalman em uma dimensão. O principal objetivo deste capítulo é explicar o conceito do Filtro de Kalman de uma forma simples e intuitiva, sem usar ferramentas que podem parecer complexas e confusas.
Avançaremos para as equações do Filtro de Kalman passo a passo.
Como mencionei anteriormente, o Filtro de Kalman é baseado em cinco equações. Já estamos familiarizados com duas delas:
Neste capítulo, iremos derivar outras três equações do filtro de Kalman.
Vamos relembrar nosso primeiro exemplo (medição do peso da barra de ouro), fizemos várias medições e calculamos a estimativa pela média.
Chegamos no seguinte resultado:
No gráfico acima, você pode ver o valor real, o valor estimado e as medições, vs. o número de medições.
As diferenças entre as medições (amostras azuis) e o valor verdadeiro (linha verde) são erros de medição. Uma vez que os erros de medição são aleatórios, podemos descrevê-los pela variância (\ (\sigma^{2} \)). A variação dos erros de medição pode ser fornecida pelo fornecedor da balança ou pode ser derivada pelo procedimento de calibração. A variação dos erros de medição é na verdade a incerteza de medição.
Vamos denotar a incerteza de medição por \(r \).
A diferença entre a estimativa (linha vermelha) e o valor verdadeiro (linha verde) é o erro de estimativa. Como você pode ver, o erro de estimativa torna-se cada vez menor à medida que fazemos mais medições, e ele converge para zero, enquanto o valor estimado converge para o valor verdadeiro. Não sabemos qual é o erro de estimativa, mas podemos estimar a incerteza na estimativa.
Vamos denotar a incerteza da estimativa por \(p \).
Vamos dar uma olhada nas medidas de peso FDP (Função de Densidade de Probabilidade).
No gráfico a seguir, podemos ver dez medidas do peso da barra de ouro.
Como você pode ver, 8 entre 10 medições estão próximas o suficiente do valor verdadeiro, portanto, o valor verdadeiro está dentro dos limites de \( 1 \sigma \).
A incerteza de medição ( \( r \) ) é a variância da medição ( \(\sigma^{2} \) ).
Vamos derivar a terceira equação que é a Equação de Ganho de Kalman . Agora, vou apresentar a derivação intuitiva da Equação de ganho de Kalman. A derivação matemática será mostrada nos próximos capítulos.
Em um filtro de Kalman, os parâmetros \( \alpha \) - \( \beta \) (- \( \gamma \) ) são calculados dinamicamente para cada iteração do filtro. Esses parâmetros são chamados de Ganho de Kalman e denotados por \( K_{n} \).
A Equação de ganho de Kalman é a seguinte:
| \( p_{n,n-1} \) | é a incerteza da estimativa extrapolada |
| \( r_{n} \) | é a incerteza da medição |
O ganho de Kalman é um número entre zero e um:
\[ 0 \leq K_{n} \leq 1 \]
Vamos reescrever a equação de atualização de estado:
Como você pode ver, o ganho de Kalman \( \left( K_{n} \right) \) é o peso que damos à medição. E \( \left(1-K_{n} \right) \) é o peso que damos à estimativa.
Quando a incerteza da medição é muito grande e a incerteza da estimativa muito pequena, o Ganho de Kalman é próximo a zero. Consequentemente, damos um grande peso à estimativa e um pequeno peso à medição.
Por outro lado, quando a incerteza da medição é muito pequena e a incerteza da estimativa muito grande, o Ganho de Kalman é próximo a um. Consequentemente, damos um peso pequeno à estimativa e um peso grande à medição.
Se a incerteza da medição for igual à incerteza da estimativa, o ganho de Kalman será igual a 0.5.
A equação de Ganho de Kalman é a terceira equação do filtro de Kalman.
A seguinte equação define a atualização da incerteza estimada:
| \( K_{n} \) | é o ganho de Kalman |
| \( p_{n,n-1} \) | é a incerteza da estimativa que foi calculada durante a estimativa do filtro anterior |
| \( p_{n,n} \) | é a estimativa de incerteza do estado atual |
Esta equação atualiza a incerteza estimada do estado atual. É chamada de Equação de Atualização da Covariância. Por que covariância? Veremos isso nos próximos capítulos.
É bastante claro a partir da equação que a incerteza da estimativa é sempre menor a cada iteração do filtro, uma vez que \(\left (1-K_ {n} \right) \leq 1 \). Quando a incerteza da medição é grande, o ganho de Kalman será baixo, portanto, a convergência da incerteza da estimativa seria lenta. No entanto, quando a incerteza da medição é pequena, o ganho de Kalman será alto e a incerteza da estimativa convergirá rapidamente para zero.
A Equação de Atualização de Covariância é a quarta equação do Filtro de Kalman.
Assim como a extrapolação de estado, a extrapolação da incerteza estimada é feita com as equações do modelo dinâmico.
Em nosso segundo exemplo, no caso do radar unidimensional, a posição alvo prevista é:
isto é, a posição prevista é igual à posição estimada atual mais a velocidade estimada atual multiplicada pelo tempo. A velocidade prevista é igual à estimativa da velocidade atual (assumindo o modelo de velocidade constante).
A estimativa de extrapolação da incerteza seria:
| \( p^{x} \) | é a incerteza da estimativa da posição |
| \( p^{v} \) | é a incerteza da estimativa de velocidade |
ou seja, a incerteza da estimativa da posição prevista é igual à incerteza da estimativa da posição atual mais a incerteza da estimativa da velocidade atual multiplicada pelo tempo ao quadrado. A incerteza da estimativa da velocidade prevista é igual à incerteza da estimativa da velocidade atual (assumindo o modelo de velocidade constante).
Em nosso primeiro exemplo (medição do peso da barra de ouro), a dinâmica do sistema é constante. Assim, a estimativa de extrapolação da incerteza seria:
| \( p \) | é a incerteza da estimativa do peso da barra de ouro |
A equação de extrapolação da incerteza estimada é chamada de Equação de Extrapolação de Covariância e esta é a quinta equação do Filtro de Kalman.
Neste capítulo, vamos combinar todas as peças em um único algoritmo. Como o filtro \( \alpha \), \(\beta \), (\(\gamma \) ), o filtro Kalman utiliza o algoritmo "Medir, Atualizar, Prever".
O gráfico a seguir fornece uma descrição esquemática de baixo nível do algoritmo:
As entradas do filtro são:
A inicialização foi realizada apenas uma vez e fornece dois parâmetros:
Os parâmetros de inicialização podem ser fornecidos por outro sistema, outro processo (por exemplo, processo de busca no radar) ou suposição baseada na experiência ou conhecimento teórico. Mesmo que os parâmetros de inicialização não sejam precisos, o filtro de Kalman poderá convergir próximo ao valor real.
A medição é realizada para cada ciclo de filtro e fornece dois parâmetros:
Além do valor medido, o filtro de Kalman requer os parâmetros de incerteza de medição. Normalmente, este parâmetro é fornecido pelo fornecedor do equipamento ou pode ser obtido pela calibração do equipamento de medição. A incerteza da medição do radar depende de vários parâmetros, como SNR (Relação Sinal Ruído), largura do feixe, largura de banda, tempo no alvo, estabilidade do relógio e muito mais. Cada medição de radar tem SNR diferente, largura do feixe e tempo no alvo. Portanto, esse radar calcula a incerteza de medição para cada medição e relata ao rastreador.
As saídas do filtro são:
Além da Estimativa do Estado do Sistema, o Filtro de Kalman também fornece Estimativa da Incerteza! Temos o mérito da precisão da estimativa. Como já mencionei, a incerteza da estimativa é dada por:
\[ p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \]
e \( p_{n,n} \) está sempre ficando menor a cada iteração do filtro, uma vez que \( \left( 1-K_{n} \right) \leq 1 \)
Portanto, cabe a nós decidir quantas medições tomar. Se estamos medindo a altura do edifício, e estamos interessados na precisão de 3 centímetros (\ (\sigma \)), faremos as medições até que a incerteza da estimativa (\ (\sigma^{2} \)) seja menor que 9 centímetros.
A tabela a seguir resume as cinco equações do Filtro de Kalman.
| Equação | Nome da Equação | Nomes alternativos usados na literatura |
|---|---|---|
| \( \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right) \) | Atualização de estado | Equação de Filtragem |
|
\( \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \) \( \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \) (Para dinâmica de velocidade constante) |
Extrapolação de Estado |
Equação de Predição Equação de Transição Equação de Previsão Modelo Dinâmico Modelo Espaço de Estados |
| \( K_{n}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \) | Ganho de Kalman | Equação de Peso |
| \( p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \) | Atualização da Covariância | Equação de Correção |
|
\( p_{n+1,n}= p_{n,n} \) (Para dinâmica constante) |
Extrapolação da Covariância | Equação Preditora da Covariância |
A figura a seguir fornece uma descrição detalhada do diagrama de blocos do Filtro de Kalman.
Conforme mencionado acima, a inicialização foi realizada apenas uma vez e fornece dois parâmetros:
A inicialização é seguida por previsão.
O processo de medição deve fornecer dois parâmetros:
O processo de atualização de estado é responsável pela estimativa do estado atual do sistema.
As entradas do processo de atualização de estado são:
Com base nas entradas, o processo de atualização de estado calcula o ganho de Kalman e fornece duas saídas:
Esses parâmetros são as saídas do Filtro Kalman.
O processo de previsão extrapola o estado atual do sistema e a incerteza da estimativa do estado atual do sistema para o próximo estado do sistema, com base no modelo dinâmico do sistema.
Na primeira iteração do filtro, as saídas de inicialização são tratadas como Estimativa de Estado Anterior e Incerteza.
Nas próximas iterações do filtro, as saídas de previsão tornam-se a Estimativa do Estado Anterior e a Incerteza.
O Ganho de Kalman define um peso da medição e um peso da estimativa anterior ao formar uma nova estimativa.
Uma baixa incerteza de medição em relação à incerteza estimada resultaria em um alto ganho de Kalman (próximo a 1). Como resultado, a nova estimativa ficaria próxima da medição. A figura a seguir ilustra a influência do alto ganho de Kalman na estimativa na aplicação de rastreamento de aeronaves.
Uma alta incerteza de medição em relação à incerteza estimada resultaria em um baixo ganho de Kalman (próximo a 0). Com isso, a nova estimativa ficaria próxima da estimativa anterior. A figura a seguir ilustra a influência do baixo Ganho de Kalman na estimativa na aplicação de rastreamento de aeronaves.
Agora, entendemos o algoritmo do Filtro de Kalman e estamos prontos para o primeiro exemplo numérico.
Sabemos com certeza que a altura do edifício não muda com o tempo, pelo menos durante o curto processo de medição.
We know for sure, that the building height doesn’t change over time, at least during the short measurement process.
Pode-se estimar a altura do edifício simplesmente olhando para ele.
A altura estimada do edifício é:
\[ \hat{x}_{0,0}=60m \]
Agora devemos inicializar a incerteza da estimativa. O erro de estimativa humano (desvio padrão) é de cerca de 15 metros: \ (\sigma = 15 \). Consequentemente, a variância é 225: \(\sigma^{2} = 225 \).
\[ p_{0,0}=225m^{2} \]
Agora, devemos prever o próximo estado com base nos valores de inicialização.
Como o modelo dinâmico do sistema de uso é constante, ou seja, o edifício não muda sua altura, então:
\[ \hat{x}_{1,0}=\hat{x}_{1,0}= 60m \]
A incerteza da estimativa extrapolada (variância) também não muda:
\[ p_{1,0}= p_{0,0}=225m^{2} \]
A primeira medição é: \( z_{1}=49.03m \).
Uma vez que o desvio padrão (\ (\sigma \)) do erro de medição do altímetro é 5, a variância (\ (\sigma ^ {2} \)) seria 25, portanto, a incerteza de medição é: \ (r_{1} = 25m^{2} \).
Cálculo de Ganho de Kalman:
\[ K_{1}= \frac{p_{1,0}}{p_{1,0}+r_{1}}= \frac{225}{225+25}=0.9 \]
Estimando o estado atual:
\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ K_{1} \left( z_{1}- \hat{x}_{1,0} \right) =60+0.9 \left( 49.03-60 \right) =50.13m \]
Atualizando a incerteza da estimativa atual:
\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.9 \right) 225=22.5m^{2} \]
Uma vez que o modelo dinâmico do sistema de uso é constante, ou seja, o edifício não muda sua altura, então:
\[ \hat{x}_{2,1}=\hat{x}_{1,1}= 50.13m \]
A incerteza da estimativa extrapolada (variância) também não muda:
\[ p_{2,1}= p_{1,1}=22.5m^{2} \]
Após um atraso de tempo unitário, a estimativa prevista da iteração anterior torna-se uma estimativa anterior na iteração atual:
\[ \hat{x}_{2,1}=50.13m \]
A incerteza da estimativa extrapolada torna-se a incerteza da estimativa anterior:
\[ p_{2,1}= 22.5m^{2} \]
A segunda medição é: \( z_{2}=48.44m \)
A incerteza de medição é: \( r_{2}=25m^{2} \)
Cálculo de ganho de Kalman:
\[ K_{2}= \frac{p_{2,1}}{p_{2,1}+r_{2}}= \frac{22.5}{22.5+25}=0.47 \]
Estimando o estado atual:
\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ K_{2} \left( z_{2}- x_{2,1} \right) =50.13+0.47 \left( 48.44-50.13 \right) =49.33m \]
Atualizando a incerteza da estimativa atual:
\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.47 \right) 22.5=11.84m^{2} \]
Uma vez que o modelo dinâmico do sistema de uso é constante, ou seja, o edifício não muda sua altura, então:
\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 49.33m \]
A incerteza da estimativa extrapolada (variância) também não muda:
\[ p_{3,2}= p_{2,2}=11.84m^{2} \]
Os cálculos para as próximas iterações são resumidos na próxima tabela:
| \( n \) | \( z_{n} \) | Estimativas do estado atual ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | Predição ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 3 | \( 55.21m \) | \[ K_{3}= \frac{11.84}{11.84+25}=0.32 \] \[ \hat{x}_{3,3}=~ 49.33+0.32 \left( 55.21 -49.33 \right) =51.22m \] \[ p_{3,3}= \left( 1-0.32 \right) 11.84=8.04m^{2} \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.22m \] \[ p_{4,3}= p_{3,3}=8.04m^{2} \] |
| 4 | \( 49.98m \) | \[ K_{4}= \frac{8.04}{8.04+25}=0.24 \] \[ \hat{x}_{4,4}= 51.22+0.24 \left( 49.98 -51.22 \right) =50.92m \] \[ p_{4,4}= \left( 1-0.24 \right) 8.04=6.08m^{2} \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=50.92m \] \[ p_{5,4}= p_{4,4}=6.08m^{2} \] |
| 5 | \( 50.6m \) | \[ K_{5}= \frac{6.08}{6.08+25}=0.2 \] \[ \hat{x}_{5,5}= 50.92+0.2 \left( 50.6 -50.92 \right) =50.855m \] \[ p_{5,5}= \left( 1-0.2 \right) 6.08=4.89m^{2} \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=50.855m \] \[ p_{6,5}= p_{5,5}=4.89m^{2} \] |
| 6 | \( 52.61m \) | \[ K_{6}= \frac{4.89}{4.89+25}=0.16 \] \[ \hat{x}_{6,6}=~ 50.855+0.16 \left( 52.61 -50.855 \right) =51.14m \] \[ p_{6,6}= \left( 1-0.16 \right) 4.89=4.09m^{2} \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=51.14m \] \[ p_{7,6}= p_{6,6}=4.09m^{2} \] |
| 7 | \( 45.87m \) | \[ K_{7}= \frac{4.09}{4.09+25}=0.14 \] \[ \hat{x}_{7,7}= 51.14+0.14 \left( 45.87 -51.14 \right) =50.4m \] \[ p_{7,7}= \left( 1-0.14 \right) 4.09=3.52m^{2} \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=50.4m \] \[ p_{8,7}= p_{7,7}=3.52m^{2} \] |
| 8 | \( 42.64m \) | \[ K_{8}= \frac{3.52}{3.52+25}=0.12 \] \[ \hat{x}_{8,8}= 50.4+0.12 \left( 42.64 -50.4 \right) =49.44m \] \[ p_{8,8}= \left( 1-0.12 \right) 3.52=3.08m^{2} \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.44m \] \[ p_{9,8}= p_{8,8}=3.08m^{2} \] |
| 9 | \( 48.26m \) | \[ K_{9}= \frac{3.08}{3.08+25}=0.11 \] \[ \hat{x}_{9,9}= 49.44 + 0.11 \left( 48.26 - 49.44 \right) = 49.31m \] \[ p_{9,9}= \left( 1-0.11 \right) 3.08=2.74m^{2} \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9} = 49.31m \] \[ p_{10,9}= p_{9,9}=2.74m^{2} \] |
| 10 | \( 55.84m \) | \[ K_{10}= \frac{2.74}{2.74+25}=0.1 \] \[ \hat{x}_{10,10}= 49.31 + 0.1 \left( 55.84 - 49.31 \right) = 49.96m \] \[ p_{10,10}= \left( 1-0.1 \right) 2.74=2.47m^{2} \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=49.96m \] \[ p_{11,10}= p_{10,10}=2.47m^{2} \] |
Em primeiro lugar, queremos garantir a convergência do Filtro de Kalman. O Ganho de Kalman deve diminuir gradualmente até atingir um estado estacionário. Quando o Ganho de Kalman é baixo, o peso das medições ruidosas também é baixo. O gráfico a seguir descreve o Ganho de Kalman durante as primeiras cem iterações do Filtro de Kalman.
Podemos ver uma redução significativa no Ganho de Kalman durante as primeiras dez iterações. O Ganho de Kalman entra em estado estacionário após aproximadamente cinquenta iterações.
Também queremos avaliar a precisão. A precisão indica quão próxima a medição está do valor verdadeiro. O gráfico a seguir compara o valor verdadeiro, os valores medidos e as estimativas para as primeiras 50 iterações.
Um erro de estimativa é a diferença entre os valores verdadeiros (a linha verde) e as estimativas do FK (a linha vermelha). Podemos ver que os erros de estimativa do nosso FK diminuem na região de convergência do filtro.
Critérios de precisão podem ser definidos com base nos requisitos específicos da aplicação. Os critérios típicos de precisão são:
Outro parâmetro importante é a incerteza da estimativa. Queremos que as estimativas do FK sejam precisas; portanto, estamos interessados em uma baixa incerteza de estimativa.
Suponha que, para uma aplicação de medição da altura de um edifício, haja um requisito de 95% de confiança. O gráfico a seguir mostra as estimativas do FK e os valores verdadeiros com intervalos de confiança de 95%.
Os intervalos de confiança são calculados com base nas incertezas das estimativas. Você pode encontrar as diretrizes para o cálculo de intervalos de confiança aqui.
No gráfico acima, os intervalos de confiança são adicionados às estimativas (a linha vermelha). 95% das amostras verdes devem estar dentro da região de confiança de 95%.
Podemos ver que a incerteza é muito alta. Vamos diminuir o parâmetro de incerteza da medição.
O gráfico a seguir descreve a saída do FK para um parâmetro de baixa incerteza de medição.
Embora tenhamos reduzido a incerteza das estimativas, muitas amostras verdes estão fora da região de confiança de 95%. O Filtro de Kalman está excessivamente confiante e demasiado otimista quanto à sua precisão.
Vamos encontrar a incerteza de medição que produz a incerteza de estimativa desejada.
O gráfico acima mostra que 2 das 50 amostras excedem ligeiramente a região de confiança de 95%. Esse desempenho atende aos nossos requisitos.
Neste exemplo, medimos a altura do edifício utilizando o Filtro de Kalman unidimensional. Diferentemente do filtro \( \alpha -\beta -(\gamma) \), o Ganho de Kalman é dinâmico e depende da precisão do dispositivo de medição.
O valor inicial utilizado pelo Filtro de Kalman não é preciso. Portanto, o peso da medição na Equação de Atualização do Estado é alto e a incerteza da estimativa é elevada.
A cada iteração, o peso da medição é menor; portanto, a incerteza da estimativa também é reduzida.
A saída do Filtro de Kalman inclui a estimativa e a incerteza da estimativa.