Filtro de Kalman (Kalman Filter)

A necessidade de predição

Começamos formulando o problema para entender por que precisamos de um algoritmo para estimação e predição de estado.

Para ilustrar, considere o exemplo de um radar de rastreamento:

Suponha que temos um radar que rastreia uma aeronave. Nesse cenário, a aeronave é o sistema, e a grandeza a ser estimada é sua posição, que representa o estado do sistema.

O radar amostra o alvo direcionando um feixe estreito até ele e fornece medições de posição da aeronave. Com base nessas medições, podemos estimar o estado do sistema (a posição da aeronave).

Para rastrear a aeronave, o radar deve revisitar o alvo em intervalos regulares apontando o feixe em sua direção. Isso significa que o radar precisa prever a posição futura da aeronave para o próximo feixe. Se não fizer isso, o feixe pode ser apontado na direção errada, resultando em perda de rastreamento. Para fazer essa predição, precisamos de conhecimento sobre como a aeronave se move. Em outras palavras, precisamos de um modelo que descreva o comportamento do sistema ao longo do tempo, conhecido como modelo dinâmico (Dynamic Model).

Para simplificar o exemplo, consideremos um mundo em uma dimensão no qual a aeronave se move ao longo de uma linha reta, aproximando-se do radar ou afastando-se dele.

O estado do sistema é definido como o alcance da aeronave em relação ao radar, denotado por \( r \). O radar envia um pulso em direção à aeronave, que é refletido pelo alvo e retorna ao radar. Medindo o tempo entre a transmissão e a recepção do pulso e sabendo que o pulso é uma onda eletromagnética que viaja à velocidade da luz, o radar pode calcular facilmente o alcance \( r \) da aeronave. Além do alcance, o radar também pode medir a velocidade \( v \) da aeronave, assim como um radar policial detecta a velocidade de um carro usando o efeito Doppler.

Suponha que no instante \( t_{0} \) o radar meça o alcance e a velocidade da aeronave com altíssima exatidão e precisão. O alcance medido é 10.000 metros e a velocidade é 200 metros por segundo. Isso nos dá o estado do sistema:

\[ r_{t_{0}} = 10,000m \]

O próximo passo é prever o estado do sistema no instante \( t_{1}=t_{0}+\Delta t \), onde \( \Delta t \) é o tempo de revisita do alvo. Dado que se espera que a aeronave mantenha velocidade constante, pode-se usar um modelo dinâmico de velocidade constante para prever sua posição futura.

A distância percorrida durante o intervalo de tempo \( \Delta t \) é dada por:

\[ \Delta r = v \cdot \Delta t \]

Assumindo um intervalo de amostragem de 5 segundos, a posição prevista no instante \( t_{1} \) é:

\[ r_{t_{1}} = r_{t_{0}} + \Delta r = 10,000 + 200 \cdot 5 = 11,000m \]

Este é um algoritmo elementar baseado em princípios simples. O estado atual do sistema é derivado da medição (Measurement) e o modelo dinâmico é usado para prever o estado futuro.

Na vida real, as coisas são mais complexas. Primeiro, as medições do radar não são perfeitamente precisas. Elas são afetadas por ruído e contêm um certo nível de aleatoriedade. Se dez radares diferentes medissem o alcance da aeronave no mesmo instante, eles produziriam dez resultados ligeiramente diferentes. Esses resultados provavelmente estariam próximos uns dos outros, mas não idênticos. A variação nas medições é causada pelo ruído de medição.

Isso leva a uma nova questão: quão certa é nossa estimativa? Precisamos de um algoritmo que não apenas forneça uma estimativa, mas também nos diga quão confiável ela é.

Outra questão é a exatidão do modelo dinâmico. Embora possamos assumir que a aeronave se move a velocidade constante, fatores externos como o vento podem introduzir desvios dessa suposição. Essas influências imprevisíveis são chamadas de ruído do processo (Process Noise).

Assim como queremos avaliar a certeza da nossa estimativa baseada em medição, também queremos entender o nível de confiança da nossa predição.

O Filtro de Kalman é um algoritmo de estimação de estado que fornece tanto uma estimativa do estado atual quanto uma predição do estado futuro, juntamente com uma medida de sua incerteza. Além disso, é um algoritmo ótimo que minimiza a incerteza de estimação de estado. É por isso que o Filtro de Kalman se tornou um algoritmo tão amplamente usado e confiável.

Exemplo de Filtro de Kalman

Comecemos com um exemplo simples: um radar em uma dimensão que mede alcance e velocidade transmitindo um pulso em direção a uma aeronave e recebendo o eco refletido. O atraso entre a transmissão do pulso e a recepção do eco fornece informação sobre o alcance \(r\) da aeronave, e o desvio de frequência do eco refletido fornece informação sobre a velocidade \(v\) da aeronave (efeito Doppler).

Neste exemplo, o estado do sistema é descrito tanto pelo alcance \(r\) quanto pela velocidade \(v\) da aeronave. Definimos o estado do sistema pelo vetor \(\boldsymbol{x}\), que inclui ambas as grandezas:

\[ \boldsymbol{x}=\left[\begin{matrix}r\\v\\\end{matrix}\right] \]

Denotamos vetores por letras minúsculas em negrito e matrizes por letras maiúsculas em negrito.

Como o estado do sistema inclui mais de uma variável, usamos ferramentas de álgebra linear, como vetores e matrizes, para descrever a matemática do Filtro de Kalman. Se você não estiver confortável com álgebra linear, revise a seção Filtro de Kalman Unidimensional no tutorial online ou no livro. Ela apresenta as equações do Filtro de Kalman e sua derivação usando matemática de nível escolar, juntamente com quatro exemplos totalmente resolvidos.

Iteração 0

Inicialização do filtro

Neste exemplo, usaremos a primeira medição para inicializar o Filtro de Kalman (para mais informações sobre técnicas de inicialização e seu impacto no desempenho do Filtro de Kalman, consulte o Capítulo 21 do livro). No instante \(t_0\), o radar mede um alcance de \(10{,}000m\) e uma velocidade de \(200m/s\). As medições são denotadas pela letra \(\boldsymbol{z}\).
Empilhamos as medições no vetor de medição \(\boldsymbol{z}\):

\[ \boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

O subscrito \(0\) indica o instante \(t_0\).

A medição não reflete exatamente o estado do sistema. As medições são corrompidas por ruído aleatório; portanto, cada medição é uma variável aleatória (random variable).

Podemos confiar nessa medição? Quão certa ela é? Cada medição é acompanhada por uma incerteza de medição (Measurement Uncertainty) ao quadrado (às vezes chamada de erro de medição). Essa incerteza ao quadrado é a variância da medição. Você pode ler mais sobre variância na seção Fundamentos Essenciais I. Para uma discussão mais detalhada sobre incerteza de medição, veja a seção Filtro de Kalman Unidimensional.

Em sistemas de radar, a incerteza de medição é largamente determinada pela razão entre a potência do sinal recebido e o ruído. Quanto maior a relação sinal-ruído (signal-to-noise ratio, SNR), menor a variância da medição e maior a nossa confiança na medição.

A figura a seguir compara casos de baixo e alto sinal na presença de ruído.

Suponha que o desvio padrão da medição de alcance seja \( 4m \) e o desvio padrão da medição de velocidade seja \( 0{,}5m/s \). Como a variância é o quadrado do desvio padrão, a incerteza de medição ao quadrado (denotada por \( \boldsymbol{R} \)) é:

\[ \boldsymbol{R}_0=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

\( \boldsymbol{R} \) é uma matriz de covariância. Os elementos da diagonal principal contêm as variâncias, e os elementos fora da diagonal são as covariâncias entre medições.

\[ \boldsymbol{R}=\left[\begin{matrix}\sigma_r^2&\sigma_{rv}^2\\[0.5em]\sigma_{vr}^2&\sigma_v^2\\\end{matrix}\right] \]

Neste exemplo, assumimos que os erros nas medições de alcance e velocidade não estão relacionados entre si; portanto, os elementos fora da diagonal da matriz de covariância de medição são definidos como zero.

Para revisar variância e desvio padrão, consulte a seção Fundamentos Essenciais I do tutorial online.
Para revisar matrizes de covariância, consulte a seção Fundamentos Essenciais II.

Durante a inicialização, a única informação que temos é uma única medição. Neste exemplo, a medição e o estado do sistema são descritos pelas mesmas grandezas (\(r\) e \(v\)). Por isso, podemos usar a medição como a estimativa inicial do estado do sistema. Isso só pode ser feito durante a etapa de inicialização:

\[ \boldsymbol{\hat{x}}_{0,0}=\boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

O subscrito \(0,0\) tem o seguinte significado:

• O primeiro índice refere-se ao tempo do sistema, que neste exemplo é \(t_0\).
• O segundo índice refere-se ao instante em que a estimativa foi feita, que também é \(t_0\).

Em outras palavras, a estimativa é para o instante \(t_0\), e também foi calculada no instante \(t_0\).

Predição

Agora prevemos o próximo estado. Suponha que o tempo de revisita do alvo seja 5 segundos \((\Delta t=5s)\); portanto, \(t_1=5s\).

Para estimar o estado futuro do sistema, devemos descrever como o sistema evolui ao longo do tempo. Neste exemplo, assumimos um modelo dinâmico de velocidade constante (o modelo de movimento):

\[ v_{1} = v_{0} = v \] \[ r_{1} = r_{0} + v_{0}\Delta t \]

(Para exemplos de modelos dinâmicos com aceleração, consulte o Capítulo 9 do livro.)

Descrevamos o modelo dinâmico em forma matricial:

\[ {\hat{\boldsymbol{x}}}_{1,0}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{0,0} \]

O subscrito \(1,0\) tem o seguinte significado:

• O primeiro índice refere-se ao tempo do sistema, que é \(t_1\).
• O segundo índice refere-se ao instante em que a estimativa foi feita, que é \(t_0\).

Assim, \( \hat{\boldsymbol{x}}_{1,0} \) é nossa estimativa do estado do sistema no instante \(t_1\), calculada usando informações disponíveis em \(t_0\). Em outras palavras, é uma predição do estado futuro.

A matriz \( \boldsymbol{F} \) é chamada de matriz de transição de estado (state transition matrix) e descreve como o estado do sistema evolui ao longo do tempo:

\[ {\hat{\boldsymbol{x}}}_{1,0}=\left[\begin{matrix}{\hat{r}}_{1,0}\\{\hat{v}}_{1,0}\\\end{matrix}\right]=\left[\begin{matrix}1&\Delta t\\0&1\\\end{matrix}\right]\left[\begin{matrix}{\hat{r}}_{0,0}\\{\hat{v}}_{0,0}\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}10,000\\200\\\end{matrix}\right]=\left[\begin{matrix}11,000\\200\\\end{matrix}\right] \]

O Apêndice C do livro descreve um método para modelar a dinâmica de qualquer sistema linear.

A equação

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} \]

é a Equação de Extrapolação de Estado (State Extrapolation Equation, predição). Ela nos diz como calcular o próximo estado a partir do atual. Ela usa nossa estimativa de estado atual e o modelo de movimento do sistema para prever o estado no próximo passo de tempo.

A forma completa da Equação de Extrapolação de Estado é:

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]

onde:

• \(\boldsymbol{u}_{n}\) é uma variável de entrada (input)
• \(\boldsymbol{G}\) é uma matriz de transição de entrada (input transition matrix)

O vetor de entrada representa informação adicional fornecida ao Filtro de Kalman, como leituras de um acelerômetro embarcado.

Neste exemplo simples, assumimos que não há entrada, então \(\boldsymbol{u}_n=0\).

Para um exemplo que inclui um termo de entrada, veja a página Equação de Extrapolação de Estado do tutorial online ou o Exemplo 10 totalmente resolvido no livro.

Toda medição e toda estimativa no Filtro de Kalman vêm acompanhadas de informação de incerteza. Após prever o próximo estado, também devemos perguntar: quão precisa é essa predição?

A incerteza ao quadrado da estimativa do estado atual é representada pela matriz de covariância:

\[ \boldsymbol{P}_{0,0}=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

No entanto, a covariância da predição não é calculada como:

\[ \textcolor{red}{\xcancel{\textcolor{black}{ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0} }}} \]

Isso ocorre porque \(\boldsymbol{P}\) é uma matriz de covariância e variâncias e covariâncias envolvem termos ao quadrado.

A Equação de Extrapolação de Covariância (sem o ruído do processo) é dada por:

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T \]

Você pode encontrar a derivação completa na seção Equação de Extrapolação de Covariância do tutorial online.

Para o nosso exemplo:

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\1.25&0.25\\\end{matrix}\right]=\left[\begin{matrix}\colorbox{yellow}{$22.25$}&1.25\\1.25&\colorbox{yellow}{$0.25$}\\\end{matrix}\right] $$

Observe a diagonal principal da matriz de covariância.

A variância da velocidade \(\sigma_v^2\) ainda é \(0{,}25 \, m^2/s^2\). Ela não mudou porque o modelo dinâmico assume velocidade constante.

Em contraste, a variância do alcance \(\sigma_r^2\) aumentou de \(16m^2\) para \(22{,}25m^2\). Isso reflete o fato de que a incerteza na velocidade leva ao aumento da incerteza no alcance ao longo do tempo.

Como observado anteriormente, a suposição de dinâmica de velocidade constante não é totalmente precisa. Na realidade, a velocidade da aeronave pode ser afetada por fatores externos e desconhecidos, como o vento. Como resultado, a incerteza real da predição é maior do que a prevista pelo modelo simples.

Essas influências imprevisíveis são chamadas de ruído do processo (Process Noise) e são denotadas por \(\boldsymbol{Q}\). Para levar esses efeitos em conta, adicionamos \(\boldsymbol{Q}\) à equação da covariância de predição:

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]

Para ganhar intuição sobre como o ruído do processo afeta o desempenho do Filtro de Kalman, veja o Exemplo 6 no tutorial online.

Suponha que o desvio padrão da aceleração aleatória seja \(\sigma_a=0{,}2m/s^2\). Isso representa incerteza na aceleração aleatória da aeronave causada por influências ambientais imprevisíveis.

Consequentemente, a variância da aceleração aleatória é \(\sigma_a^2=0{,}04m^2/s^4\).

Para o nosso exemplo, a matriz de ruído do processo é dada por:

$$ \boldsymbol{Q} = \left[\begin{matrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 \end{matrix}\right] \sigma_a^2 $$

Com \(\Delta t=5\mathrm{s}\) e \(\sigma_a^2=0{,}04\,\mathrm{m}^2/\mathrm{s}^4\), isso se torna:

$$ \boldsymbol{Q}=\left[\begin{matrix}\frac{625}{4}&\frac{125}{2}\\[0.5em] \frac{125}{2}&25\\\end{matrix}\right]0.04=\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right] $$

A derivação da matriz de ruído do processo é apresentada na Seção 8.2.2 do livro.

Após adicionar o ruído do processo, a incerteza ao quadrado da nossa predição é:

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T+\boldsymbol{Q}\ =\left[\begin{matrix}22.25&1.25\\1.25&0.25\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]\ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] $$

Resumo da Iteração 0

• Inicialização
  Inicializamos o Filtro de Kalman usando a primeira medição como a estimativa inicial de estado \( {\hat{\boldsymbol{x}}}_{0,0} \) e a covariância da medição como a covariância inicial do estado \(\boldsymbol{P}_{0,0}\).
  Observe que isso só pode ser feito durante a fase de inicialização.
• Predição
  Previmos o estado e sua incerteza no próximo passo de tempo, quando o radar revisitar a aeronave. As equações de predição do Filtro de Kalman são:
  
  
  Equação de Extrapolação de Estado (State Extrapolation Equation)
  \[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]
  Equação de Extrapolação de Covariância (Covariance Extrapolation Equation)
  \[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]
  onde:
  
  • \(\hat{\boldsymbol{x}}_{n,n}\): vetor de estado estimado no passo de tempo \(n\)
  • \(\hat{\boldsymbol{x}}_{n+1,n}\): vetor de estado previsto para o passo \(n+1\), calculado usando informações disponíveis em \(n\)
  • \(\boldsymbol{u}_n\): variável de controle/entrada, representando entradas externas conhecidas do sistema
  • \(\boldsymbol{F}\): matriz de transição de estado (state transition matrix)
  • \(\boldsymbol{G}\): matriz de entrada/controle (input/control matrix), que mapeia entradas para variáveis de estado
  • \(\boldsymbol{P}_{n,n}\): matriz de covariância (incerteza ao quadrado) do estado atual
  • \(\boldsymbol{P}_{n+1,n}\): matriz de covariância (incerteza ao quadrado) do estado previsto
  • \(\boldsymbol{Q}\): matriz de ruído do processo (Process Noise)

Iteração 1

Atualização do filtro

Considere a segunda medição em \(t_1\):

\[ \boldsymbol{z}_1=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] \]

Devido a um pico de ruído forte durante essa medição, a relação sinal-ruído (signal-to-noise ratio, SNR) é significativamente menor do que na primeira medição. Como resultado, a incerteza da segunda medição é maior.

Suponha que o desvio padrão da medição de alcance seja \(6m\) e o desvio padrão da medição de velocidade seja \(1{,}5m/s\). A correspondente matriz de covariância de medição é:

\[ \boldsymbol{R}_1=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

Queremos estimar o estado atual do sistema \(\hat{\boldsymbol{x}}_{1,1}\). No instante \(t_1\), temos duas fontes de informação:

• O estado previsto \(\hat{\boldsymbol{x}}_{1,0}\) (calculado no passo anterior); e
• A nova medição \(\boldsymbol{z}_1\)

Em qual devemos confiar?

Intuitivamente, poderíamos preferir usar a medição como a estimativa atual, isto é, \(\hat{\boldsymbol{x}}_{1,1}=\boldsymbol{z}_1\), porque ela é mais recente do que a predição.

Por outro lado, a medição também é mais ruidosa. Se compararmos os elementos da diagonal principal da covariância da predição \(\boldsymbol{P}_{1,0}\) com a covariância da medição \(\boldsymbol{R}_1\), vemos que a incerteza da predição é menor do que a incerteza da medição:

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right] \]

Então, talvez devêssemos ignorar a nova medição e manter a predição, isto é, \(\hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}\)?

Nesse caso, perdemos a informação nova fornecida pela medição atual.

A ideia-chave do Filtro de Kalman é que não fazemos nem uma coisa nem outra. Em vez disso, combinamos a predição e a medição, dando mais peso àquela com menor incerteza.

A solução é uma média ponderada entre a medição e a predição:

\[ \hat{x}_{1,1}=K_1 z_1\ +\ \left({1-\ K}_1\right){\hat{x}}_{1,0}, \quad 0\leq K_1 \leq 1 \]

Aqui, o peso \(K_1\) é o Ganho de Kalman (Kalman Gain). Ele determina quanto peso é dado à medição em relação à predição de modo a minimizar a incerteza da estimativa. É isso que torna o Filtro de Kalman um filtro ótimo (desde que o sistema e os ruídos se comportem de acordo com as suposições do modelo).

Apresentarei a equação do ganho de Kalman em breve, mas primeiro foquemos na Equação de Atualização de Estado (State Update Equation). Em forma matricial, ela é escrita como:

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + (\boldsymbol{I} - \boldsymbol{K}_1)\hat{\boldsymbol{x}}_{1,0} \]

onde \(\boldsymbol{I}\) é a matriz identidade (identity matrix; matriz quadrada com uns na diagonal principal e zeros no restante).

Reescrevamos essa equação:

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + \hat{\boldsymbol{x}}_{1,0} - \boldsymbol{K}_1\hat{\boldsymbol{x}}_{1,0}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}) \]

Essa forma mostra que o estado atualizado é a predição \(\hat{\boldsymbol{x}}_{1,0}\) mais um termo de correção \(\boldsymbol{K}_1\left(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\right)\).

A correção é proporcional à diferença entre a medição e a predição \(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\), chamada de inovação (innovation) ou residual de medição (measurement residual).

No nosso exemplo, tanto o estado do sistema quanto a medição são vetores que representam as mesmas grandezas físicas (alcance e velocidade). Portanto, podemos subtrair diretamente \(\hat{\boldsymbol{x}}_{1,0}\) de \(\boldsymbol{z}_1\).

No entanto, isso nem sempre ocorre. Em geral, a medição e o estado do sistema podem pertencer a domínios físicos diferentes. Por exemplo, um termômetro digital mede um sinal elétrico, enquanto o estado do sistema é a temperatura.

Por esse motivo, o estado previsto deve primeiro ser transformado para o domínio da medição:

\[ \boldsymbol{H} \hat{\boldsymbol{x}}_{1,0} \]

A matriz \(\boldsymbol{H}\) é chamada de matriz de observação (observation/measurement matrix). Ela mapeia as variáveis de estado para as quantidades efetivamente medidas.

No nosso exemplo, a matriz de observação é simplesmente a matriz identidade:

\[ \boldsymbol{H}=\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]=\boldsymbol{I} \]

Para mais informações sobre a matriz de observação, veja a seção Equação de Medição do tutorial online e os Exemplos 9 e 10 no livro.

Agora podemos reescrever a equação de atualização de estado como:

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

A inovação \(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}\) representa informação nova.

O ganho de Kalman determina quanto essa nova informação deve alterar o estado previsto, isto é, quão fortemente corrigimos a predição.

Caso unidimensional

No caso unidimensional, o Ganho de Kalman é dado por:

\[ K_n=\frac{p_{n,\ n-1}}{p_{n,\ n-1}+r_n} \]

onde:

• \(p_{n,\ n-1}\): variância prevista do estado
• \(r_n\): variância da medição

O ganho de Kalman é escolhido para minimizar a variância da estimativa atualizada \(p_{n,n}\), razão pela qual o Filtro de Kalman é ótimo.

Para construir intuição e ver a derivação completa no caso unidimensional, veja a seção Filtro de Kalman Unidimensional do tutorial online.

Caso multivariado

No Filtro de Kalman multivariado, o ganho de Kalman torna-se uma matriz e é dado por:

\[ \boldsymbol{K}_n=\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1} \]

Para a derivação da Equação do Ganho de Kalman multivariado, veja a seção Ganho de Kalman do tutorial online.

Vamos calcular o Ganho de Kalman para \(t_1\):

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1} \]

No nosso exemplo, \(\boldsymbol{H}=\boldsymbol{I}\) e \(\boldsymbol{H}^T=\boldsymbol{I}\).

Substitua as matrizes:

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right], \quad \boldsymbol{R}_1=\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}64.5&3.75\\3.75&3.5\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}0.0165&-0.0177\\-0.0177&0.3047\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

A estimativa de estado atualizada é:

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

No nosso exemplo, \(\boldsymbol{H}=\boldsymbol{I}\), então a inovação é simplesmente:

\[ \boldsymbol{z}_1 - \boldsymbol{I}\hat{\boldsymbol{x}}_{1,0}=\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] - \left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]=\left[\begin{matrix}20\\2\\\end{matrix}\right] \]

Agora aplique a correção:

\[ \boldsymbol{K}_1\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right] \]

Finalmente:

\[ \hat{\boldsymbol{x}}_{1,1}=\left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]+\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right]=\left[\begin{matrix}11{,}009.37\\201.43\\\end{matrix}\right] \]

Uma vez estimado o estado atual, também queremos quantificar a incerteza dessa estimativa.

Caso unidimensional

No caso unidimensional, a Equação de Atualização de Covariância é:

\[ p_{n,n}=(1-K_n)p_{n,\ n-1} \]

Para a derivação, veja a seção Filtro de Kalman Unidimensional do tutorial online.

Caso multivariado

Forma de Joseph (Joseph form)

Para o Filtro de Kalman multivariado, a equação de atualização de covariância é comumente escrita em uma forma numericamente estável, conhecida como forma de Joseph (Joseph form), introduzida por Peter Joseph.

\[ \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 \]

onde:

• \(\boldsymbol{P}_{n,n}\): covariância da estimativa de estado atualizada (posterior)
• \(\boldsymbol{P}_{n,n-1}\): covariância da estimativa de estado prevista (a priori)
• \(\boldsymbol{K}_n\): Ganho de Kalman
• \(\boldsymbol{H}\): matriz de observação (medição)
• \(\boldsymbol{R}_n\): matriz de covariância do ruído de medição
• \(\boldsymbol{I}\): matriz identidade (matriz quadrada com uns na diagonal principal e zeros no restante)

Para a derivação, veja a seção Equação de Atualização de Covariância do tutorial online.

Forma simplificada

Na literatura, você também verá com frequência a atualização de covariância na forma simplificada:

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1} \]

Para sua derivação, veja a seção Equação de Atualização de Covariância Simplificada.

Ambas as formas fornecem o mesmo resultado em aritmética exata. Contudo, para implementações computacionais, a forma de Joseph é geralmente preferida por ser mais numericamente estável.

Apenas para este exemplo, usaremos a equação de atualização de covariância simplificada:

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1\boldsymbol{H})\boldsymbol{P}_{1,0} \]

No nosso exemplo, \(\boldsymbol{H}=\boldsymbol{I}\), então:

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1)\boldsymbol{P}_{1,0} \]

Agora substitua as matrizes:

\[ \boldsymbol{P}_{1,1}=\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right] - \left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\right)\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] \]

\[ =\left[\begin{matrix}0.5952&-0.6377\\-0.0399&0.6856\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]=\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right] \]

Análise do resultado

A incerteza da estimativa atualizada é menor do que a incerteza da predição e a incerteza da medição:

\[ \boldsymbol{P}_{1,1}=\left[\begin{matrix}\colorbox{yellow}{$14.57$}&1.43\\1.43&\colorbox{yellow}{$0.71$}\\\end{matrix}\right]\ \ \ \ \ \ \boldsymbol{P}_{1,0}=\ \left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right]\ \ \ \ \ \boldsymbol{R}_\mathbf{1}=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

Ao combinar a medição com a predição e ponderá-las usando o ganho de Kalman, obtemos uma estimativa com menor incerteza.

Adicionar informação nova, mesmo quando possui alta incerteza, sempre reduz a incerteza da estimação. Veja o capítulo Fusão de Sensores no livro e os Apêndices G e H para a demonstração matemática. Do ponto de vista teórico, novas medições nunca deveriam ser ignoradas.

Na prática, entretanto, muitas vezes é necessário rejeitar certas medições. Veja o capítulo Tratamento de Outliers no livro para métodos práticos de lidar com medições não confiáveis.

Predição

A etapa de predição da Iteração 1 (de \( t_1 \) para \( t_2 \)) é idêntica à etapa de predição da Iteração 0 (de \( t_0 \) para \( t_1 \)), exceto que agora partimos da estimativa atualizada \(\hat{\boldsymbol{x}}_{1,1}\) e de \(\boldsymbol{P}_{1,1}\).

Predição de estado

\[ \hat{\boldsymbol{x}}_{2,1}=\boldsymbol{F}\hat{\boldsymbol{x}}_{1,1} \]

\[ \hat{\boldsymbol{x}}_{2,1}=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}11,009.37\\201.43\\\end{matrix}\right]=\left[\begin{matrix}12,016.5\\201.43\\\end{matrix}\right] \]

Predição de covariância

\[ \boldsymbol{P}_{2,1}=\boldsymbol{F}\boldsymbol{P}_{1,1}\boldsymbol{F}^\top + \boldsymbol{Q} \]

\[ \boldsymbol{P}_{2,1}=\ \left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]=\left[\begin{matrix}52.86&7.47\\7.47&1.71\\\end{matrix}\right] \]

Observe que ambas as variâncias aumentam novamente durante a etapa de predição. Isso ocorre porque, com a passagem do tempo sem uma nova medição, a incerteza naturalmente cresce. Em particular, a incerteza na velocidade provoca incerteza adicional no alcance, motivo pelo qual a variância do alcance aumenta mais rapidamente do que a variância da velocidade.

Resumo da Iteração 1

• Atualização
  
  • Estimamos o estado atual do sistema \(\hat{\boldsymbol{x}}_{1,1}\) como uma combinação ponderada do estado previsto \(\hat{\boldsymbol{x}}_{1,0}\) e da medição \(\boldsymbol{z}_1\).
    A ponderação é determinada pelo Ganho de Kalman \(K_1\). O Ganho de Kalman é calculado a partir da covariância do estado previsto \(\boldsymbol{P}_{1,0}\) e da covariância da medição \(\boldsymbol{R}_1\), e minimiza a incerteza da estimativa atualizada \(\boldsymbol{P}_{1,1}\).
  • As equações de atualização do Filtro de Kalman são:
    
    
    Equação de Atualização de Estado (State Update Equation)
    \[ \hat{\boldsymbol{x}}_{n,n}=\hat{\boldsymbol{x}}_{n,n-1}+\boldsymbol{K}_n\left(\boldsymbol{z}_n\ -\ \boldsymbol{H}\hat{\boldsymbol{x}}_{n,n-1}\right) \]
    Equação de Atualização de Covariância (forma de Joseph)
    \[ \boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)^T+\boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]
    Ou sua forma simplificada
    \[\boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\]
    Equação do Ganho de Kalman (Kalman Gain)
    \[ \boldsymbol{K}_n=\ \boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1}\]
  onde:
  
  • \( \hat{\boldsymbol{x}}_{n,n} \): estimativa de estado atualizada no passo de tempo n
  • \( \hat{\boldsymbol{x}}_{n,n-1} \): estado previsto no passo n, calculado com informação disponível em n-1
  • \( \boldsymbol{z}_n \): vetor de medição
  • \( \boldsymbol{P}_{n,n} \): covariância da estimativa de estado atualizada
  • \( \boldsymbol{P}_{n,n-1} \): covariância da estimativa de estado prevista
  • \( \boldsymbol{K}_n \): Ganho de Kalman
  • \( \boldsymbol{H} \): matriz de observação (medição)
  • \( \boldsymbol{R}_n \): matriz de covariância do ruído de medição
  • \( \boldsymbol{I} \): matriz identidade
• Predição
  A etapa de predição na Iteração 1 é a mesma da Iteração 0.
  Propagamos a estimativa de estado atual e sua covariância para o próximo passo de tempo, quando o radar revisitar a aeronave, usando o modelo de transição de estado.

Resumo do Exemplo