Notas: Filtros de Kalman

Protegido por Copyleft - all rights reversed.
Em particular fica excluído qualquer uso com fins lucrativos deste texto.

\[ \newcommand{\SYS}[1]{\left\lbrace\begin{array}{rcl}#1\end{array}\right.} \newcommand{\mx}[2]{\left\lbrack\begin{array}{#1}#2\end{array}\right\rbrack} \newcommand{\AT}[1]{\left(#1\right)} \newcommand{\transpose}{^{\top}} \]

Estas notas ilustram e resumem o artigo sobre Filtros de Kalman (FK) na wikipedia, grandemente baseado neste texto e que juntamente com Kalman Filtering in R são excelentes introduções a sério aos FK, no último caso complementada com imensas referências à linguagem estatística R. Não pretendem, nem podem, ser uma exposição completa, mas apenas transmitir algumas ideias iniciais que resultam da mistura da estatística com a computação.

Um Filtro de Kalman (FK) é um algoritmo que produz estimativas de variáveis ocultas com base em sequências de observações e que tende a ser mais preciso que outros algoritmos baseados unicamente nas observações.

Em particular, num FK supõe-se que é conhecida a lei da evolução das variáveis ocultas e é a aplicação desse pressuposto que aumenta a qualidade da estimativa.

Porque é que estamos interessados nos FK?
A estimação de estados parcialmente escondidos é uma capacidade crítica para robots, para outros sistemas autónomos e, em geral, um complemento necessário à análise de dados.
Além disso, os pressupostos do algoritmo FK são suficientemente compatíveis com as condicionantes das aplicações reais, para ambientes físicos.

No Contexto da Robótica

O Problema

Consideremos o problema de determinar o percurso dum robot MINDSTORM equipado com um conjunto de sensores comum nestes robots.

A informação que pretendemos conhecer é a sequência de poses

\[ \AT{p_0,q_0,\theta_0},\AT{p_1,q_1,\theta_1},\ldots \]

Para determinar o percurso dispomos de:

A pose1 , \( x = \AT{p,q,\theta} \), indica a posição do robot na sala, \( \AT{p,q} \), e para onde está voltado, \( \theta \). Esta informação sobre um robot, em geral, é considerada mínima. Isto é, usando menos do que a pose, por exemplo apenas a posição, \( \AT{p,q} \), não se distinguem situações criticamente diferentes. Para um robot perto dum obstáculo, é importante saber se está virado para o obstáculo ou de costas para este. No primeiro caso queremos enviar o “comando” recuar e no segundo avançar. Desconhecendo a orientação \( \theta \) do robot, não há forma de escolher o melhor comando.

Os controlos activam os motores e alteram a pose do robot. Os motores MINDSTORM permitem especificar o ângulo que cada motor vai rodar \( u=\AT{\alpha^L,\alpha^R} \).

Os sensores para os robots MINDSTORM são, em geral, bastante limitados; vamos supor que o robot que estamos a usar tem odómetro em cada roda, \( O^L, O^R \), um sensor de distância apontado para a frente, \( D \), e um detector de cor apontado para o chão, \( C \). Uma observação é um vector \( z = \AT{o^L, o^R, d, c} \) formado pelos valores produzidos pelos sensores.

Conforme este robot se desloca, por exemplo numa sala, os sensores produzem valores diferentes, que dependem da situação física do robot em relação à sala. As observações variam conforme o robot se desloca e portanto devemos considerar a sucessão ao longo do tempo: \[ z_1 = \AT{o^L_1, o^R_1, d_1, c_1}, z_2 = \AT{o^L_2, o^R_2, d_2, c_2}, \ldots \]

Agora podemos formular com um pouco mais de rigor o problema de determinar o percurso do robot:

Dada a pose inicial, \( x_0 \), uma sequência de comandos \( u_1,u_2,\ldots,u_n \), e uma sequência de observações \( z_1,z_2,\ldots,z_n \) qual foi o percurso \( x_1,x_2,\ldots,x_n \) percorrido pelo robot?

Os Modelos

Nesta formulação procura-se descobrir uma incógnita, o percurso, a partir dos dados conhecidos, a pose inicial, os controlos e as observações. Mas apenas esta informação é insuficiente. Por exemplo, não sabemos como é que um controlo afecta a pose do robot; não sabemos onde estão os obstáculos nem as manchas de cor no chão. É necessário algo mais: um modelo do sistema, que nos diga como é que os controlos afectam a pose e como é que cada pose determina uma observação. Isto é, precisamos de duas funções: \[ \SYS{ x' &=& F\AT{x,u}\\ z &=& H\AT{x} }. \]

A função \( x' = F\AT{x,u} \) define a evolução do robot, isto é, a nova pose, \( x' \), que resulta de aplicar o controlo \( u \) quando o robot está na pose \( x \). É nesta função que está “escondida” a informação sobre o mapa da sala e sobre as interacções entre o robot e os restantes objectos. Por exemplo, se na pose \( x \) a frente do robot estiver encostada a um obstáculo então \( x = F\AT{x,\text{avancar}} \) porque o robot está bloqueado nessa direcção (a menos que o obstáculo caia…).

Por outro lado, a função \( z = H\AT{x} \) define como os sensores captam o ambiente em que o robot está inserido.

Conforme o ambiente em que o robot interage é mais ou menos complexo, também as funções \( F \) e \( H \) serão menos ou mais simples. No extremo da máxima simplicidade temos um ambiente aborrecido, em que nada acontece e nada é observado: \( F\AT{x,u}=x_0; H\AT{x}=z_0 \).

Para ambientes mais interessantes são necessárias funções mais complexas. Felizmente obtêm-se funções bastante úteis com pouco mais complexidade, em particular quando \( F \) e \( H \) são lineares \[ \SYS{ x' = F\AT{x,u} &=& A x + B u\\ z = H\AT{x} &=& C x } \] já é possível descrever inúmeros ambientes.

Mesmo que a situação concreta em que o robot está não seja exactamente descrita por um sistema linear, em muitos casos a linearização desse sistema é suficientemente útil para evitar o recurso a sistemas mais complexos. No entanto, a linearização nem sempre funciona bem…

Uma Vaga de Realidade

Qualquer modelo representa uma idealização da realidade física e a utilidade de um modelo em particular depende de dois factores em conflito:

No caso dos sistemas lineares a questão computacional está (muito) bem resolvida: o cálculo das funções \( F \) e \( H \) resume-se à soma e produto de matrizes. No entanto aquele modelo descreve um cenário em que os motores e os sensores são perfeitos, sem erros.

Na realidade os motores têm folgas, os sensores introduzem ruído, a incerteza afecta todas as variáveis. Por exemplo, se o sensor de distância indicar \( 25,3 \textrm{cm} \) não podemos concluir que exite um obstáculo exactamente a \( 25,3 \textrm{cm} \) mas aproximadamente a essa distância.

Esta constatação indica que devemos adicionar algum ruído, \( \omega,\nu \), ao modelo que já tínhamos: \[ \SYS{ x' &=& A x + B u + \omega\\ z &=& C x + \nu } \]

Mas por enquanto ainda nada sabemos sobre a forma desse ruído. Uma suposição que reflecte uma realidade comum é que o ruído resulta de muitos factores pequenos. Por exemplo, no caso do sensor de distância o valor medido pode resultar da distância entre o sensor e o obstáculo mais alguma vibração residual dos motores do robot mais alguma variação na tensão eléctrica dos circuitos do sensor mais alguma irregularidada na superfície do obstáculo amis por partículas em suspensão entre o robot e o obstáculo mais etc, etc. Cada uma destas inúmeras “causas” é uma pequena parcela com pouca influência na soma final.

Em princípio é possível inventariar os milhões de micro-factores que influenciam o valor medido por um sensor, ou o efeito de um comando de motor na pose do robot, e alargar o modelo de forma a inclui-los. No entanto, põem-se alguns obstáculos a essa opção:

Além disso, para garantir que o modelo é exacto é um esforço inglório porque o aleatório é fisicamente inevitável: em termos físicos, ao “descer” na escala dos micro-factores acabaríamos por chegar ao fenómenos quânticos, dominadas por vários processos verdadeiramente aleatórios.

O melhor que podemos fazer é adicionar ruído adequado a um modelo ideal. Quando o ruído resulta de muitos factores pequenos, pode ser descrito por uma distribuição gaussiana, \( \mathcal{N}\AT{0,\Sigma} \), com média \( 0 \) e covariância \( \Sigma \). Portanto, adicionando ruído gaussiano ao modelo linear obtém-se

\[ \SYS{ x' &=& A x + B u + \omega;\: \omega \sim \mathcal{N}(0,Q)\\ z &=& C x + \nu;\: \nu \sim \mathcal{N}(0,R) } \] em que as covariâncias dos ruídos \( \omega,\nu \) são dadas, respectivamente, pelas matrizes \( Q \) e \( R \).

Este tipo de modelos, em muitos cenários, apresenta as duas características positivas referidas acima: representa bem a realidade e é computacionalemente acessível. Por essas duas razões está bem estudado e é muito aplicado. A formulação final do problema para determinar o percurso dum robot MINDSTORM fica então dada por

Um robot MINDSTORM vai deslocar-se numa sala descrita pelo modelo \( x'=Ax+Bu+\omega;z=Cx+\nu; \) com ruídos gaussianos \( \omega\sim\mathcal{N}\AT{0,Q}; \nu\sim\mathcal{N}\AT{0,R} \). Dada a pose inicial, \( x_0 \), uma sequência de comandos \( u_1,u_2,\ldots,u_n \), e uma sequência de observações \( z_1,z_2,\ldots,z_n \) qual foi o percurso \( x_1,x_2,\ldots,x_n \) percorrido pelo robot?

Ora, este é exactamente o tipo de problema que os FK tentam resolver.

Outra Vaga de Realidade

Consideremos uma sala com um único obstáculo. Enquanto o robot está distante do obstáculo (e das paredes) o modelo \( x'= Ax + Bu + \omega \) pode ser bastante simples: basta considerar o deslocamento relativo de cada roda, que depende apenas do ângulo que o motor roda. Porém, quando se põe a possibilidade do robot colidir com o obstáculo, o simples modelo anterior já não serve. É necessário incluir informação sobre as fronteiras do obstáculo, como estas interagem com os sensores, e o que acontece em caso de colisão.

Consideremos agora uma sala com vários obstáculos, com diferentes geometrias e propriedades de colisão. Adicionalmente, consideremos que alguns destes obstáculos são móveis.

Estas considerações devem mostrar que embora os FK sejam uma base importante para estimar a pose (ou outras variáveis) não são suficientes. É necessário um processo mais sofisticado que use os FK integrados num sistema flexível.

Enunciado

A formulação básica do FK aplica-se a sistemas discretos, lineares, invariantes no tempo (LIT) (em inglês: Linear Time Invariant (LTI)) controlados com ruído gaussiano \[ \SYS{ x' &=& A x + B u + \omega\\ \omega &\sim& \mathcal{N}(0,Q) } \] e observado com ruído gaussiano \[ \SYS{ z &=& C x + \nu\\ \nu &\sim& \mathcal{N}(0,R) }. \]

O problema consiste em calcular uma estimativa \( \hat{x}_t \) do valor “actual”, \( x_t \), do processo estocástico \( x_0, x_1, \ldots, x_t \) dado que apenas conhecemos as observações \( z_0, z_1, \ldots, z_t \), os comandos \( u_0, u_1, \ldots, u_t \) (e as estimativas anteriores \( \hat{x}_0, \ldots, \hat{x}_{t-1} \)).

Nas fórmulas acima as letras maiúsculas, \( A,B,Q,C,R \), denotam matrizes, as letras minúsculas \( x,u,z \) vectores e as letras gregas \( \omega,\nu \) ruído.

Nome Notação Função
modelo de transição \( A \) determina a evolução “pura” do sistema
modelo de controlo \( B \) define como um sinal de controlo, \( u \), afecta a evolução do sistema
covariância do processo \( Q \) é a covariância do ruído do processo, \( \omega \)
modelo de observação \( C \) define como o sistema é observado (ou medido)
covariância da observação \( R \) é a covariância do ruído da observação, \( \nu \)
estado do sistema \( x \) é o conjunto das variáveis que determinam como o sistema evolui
sinal de controlo \( u \) é um contributo “externo” para a evolução do sistema
observação \( z \) é a parte do sistema que pode ser medida
ruído do processo \( \omega \) componente estocástica da dinâmica
ruído das observações \( \nu \) erros nas medições

Os pressupostos mais importantes (e restritivos) para as aplicações dos FK são que

  1. o processo \( x' = f\AT{x,u}; z = h\AT{x} \) é linear e
  2. os ruídos \( \omega,\nu \) são gaussianos.

Para ambos os pressupostos existem inúmeras generalizações, facilmente encontradas na wikipedia.

Algoritmo FK

O algoritmo FK actualiza a estimativa do estado do sistema, \( \hat{x} \), dado

  1. a estimativa actual, \( \hat{x} \) e
  2. a observação actual, \( z \), ou
  3. o controlo actual, \( u \).

A actualização da estimativa alterna dois passos complementares:

  1. Usar a estimativa actual, \( \hat{x} \) e o controlo, \( u \), (e os modelos de transição e controlo) para prever o estado seguinte;
  2. Usar a estimativa actual, \( \hat{x} \), a observação, \( z \), (e o modelo de observação) para corrigir a estimativa;

Este método que alterna previsão e correcção é muito comum em algoritmos de estimação. No FK em particular o passo de correcção é definido de forma que minimiza o valor esperado do quadrado da magnitude do erro: \[ \mathbb{E}\left[ \left|x - \hat{x} \right|^2 \right] \]

Cálculos

Ambos os passos do algoritmo FK são apenas sequências de cálculos matriciais.

O estado do algoritmo é um par \( \AT{\hat{x},P} \) com a estimativa actual, \( \hat{x} \), e a covariância da estimativa, \( P \). O estado inicial do algoritmo é \( \AT{\hat{x}, P} = \AT{x_0, Q} \), actualizado por

  1. Prever (a priori) dado um controlo, \( u \): \[ \SYS{ \hat{x} &\gets& A \hat{x} + B u\\ P &\gets& A P A^{\top} + Q } \]

  2. Corrigir (a posteriori) dada uma observação, \( z \),

    1. efectuar cálculos auxiliares \[ \SYS{ e &=& z - C \hat{x}\\ S &=& C P C^{\top} + R\\ K &=& P C^{\top} S^{-1} } \]
    2. corrigir as estimativas do estado e da covariância: \[ \SYS{ \hat{x} &\gets& \hat{x} + Ke\\ P &\gets& \AT{\mathrm{I} - K C} P } \]

Com esta formulação o algoritmo FK

Simplificação

A aplicação sucessiva do algoritmo FK define sequências de vectores e de matrizes. Especialmente importante é a constatação que

a sequência \( K_0, K_1, \ldots, K_t \) tende a estabilizar. Isto é, para \( t \) suficientemente grande, \( K_t \approx K_{t+1} \approx \ldots \approx K_{\infty} \).

Quando esta sequência estabiliza deixa de ser necessário efectuar a maior parte dos cálculos indicados acima. Com efeito, passa a ser suficiente calcular

\[ \hat{x} \gets \hat{x} + K_{\infty} \AT{z - C\hat{x}} = K_{\infty}z + \AT{\mathrm{I}-K_{\infty}C}\hat{x} \]

Exemplos

A implementação do algoritmo FK (na formulação básica, descrita acima) não apresenta nenhuma dificuldade técnica, sendo necessário apenas:

Para completar uma biblioteca elementar sobre filtros de Kalman é ainda necessário considerar o aspecto computacional da simulação com ruído. No caso em que o sistema tem uma dimensão, \( Q \in \mathbf{R} \), é simples encontrar um gerador de números (pseudo-)aleatórios gaussianos, \( \mathcal{N}\AT{\mu,\sigma} \). Mas no caso multidimensional, \( \Sigma \in \mathbf{R}^{n\times n}, n > 1 \), pode ser necessário implementar um gerador adequado para produzir ruído \( \epsilon\sim \mathcal{N}\AT{0,\Sigma} \). Esse gerador usa a decomposição de Cholesky \( \Sigma = L L\transpose \) e a seguinte proposição: Seja \( e = \AT{e_1, \ldots, e_n} \) um vector em que cada componente \( e_i \sim \mathcal{N}\AT{0,1} \). Então \( \epsilon = L e \sim \mathcal{N}\AT{0,\Sigma} \).

Discretização de Sistemas de Equações Diferenciais Ordinárias

Ainda antes do primeiro exemplo, é necessário considerar mais um ponto importante. Frequentemente os sistemas dinâmicos surgem descritos na forma de equações diferenciais:

\[ \dot{x} = Ax + Bu \]

Esta representação descreve um sistema contínuo, e embora semelhante e relacionada, não é igual a \( x' = Ax + Bu \), que descreve um sistema discreto.

Portanto, para se estimar o estado de um sistema contínuo por FK é necessário converter as equações contínuas do sistema numa versão discreta. De várias formas possíveis, a conversão seguinte é bastante simples. Supondo que é dado o sistema contínuo \[ \dot{x} = Ax + Bu \]

  1. Fixa-se um passo no tempo, \( \delta_t \), de forma que os instantes considerados são \( t_0, t_1, \ldots, t_N \) com \( t_k = t_0 + k\delta_t \);
  2. Aproxima-se \[ \dot{x} \approx \frac{x' - x}{\delta_t} \]
  3. E portanto \[ x' \approx \delta_t \dot{x} + x \Leftrightarrow x' \approx \AT{\mathrm{I} + \delta_t A}x + \delta_t B u \]

Resumindo: O sistema contínuo \[ \dot{x} = Ax + Bu \] pode ser aproximado pelo sistema discreto \[ x' = A_1 x + B_1 u \] em que \[ \SYS{ A_1 &=& \mathrm{I} + \delta_t A \\ B_1 &=& \delta_t B } \]

O mais simples de todos os exemplos: estimar uma constante

Nenhum aparelho de medição está isento de erro, o que implica que todas as medições perturbam o “valor real” que se pretende descobrir. Num exemplo fictício, um saco de batatas numa balança “pesa” \( 22.24 \textrm{Kg} \) mas se for pesado uma segunda vez o valor é \( 22.19 \textrm{Kg} \) e se voltarmos a pesar obtemos \( 22.27 \textrm{Kg} \).

Embora seja (eventualmente) possível explicar estas flutuações em termos de fenómenos físicos como o posicionamento do saco no prato da balança, ou variações dentro do mecanismo da balança, de facto só estamos a adiar e complicar o problema inicial: “Afinal, quanto pesa o saco de batatas?”. Considerando as limitações inerentes à medição de qualquer grandeza física, sabemos que nunca vamos obter uma medição exacta. O que nos aponta para a segunda melhor solução possível: fazer a melhor estimativa desse valor.

De facto, como veremos, os FK são “demasiado sofisticados” para aplicar a um processo constante, como estimar o peso de um saco de batatas. Porém, esse exercício ilustra muito bem alguns aspectos centrais dos FK.

Então temos um saco de batatas que foi pesado 50 vezes, e os valores observados nessas 50 pesagens foram registados, e apresentados num gráfico.

plot of chunk unnamed-chunk-1

Com base nesses valores observados, temos de estimar o “peso verdadeiro” do saco. Como acabámos de aprender a usar os FK e estamos cheios de genica, vamos fazer a estimativa do “peso verdadeiro” com FK:

  1. Dispomos de 50 medições do peso do saco de batatas: \( Z_1,\ldots,Z_{50} \), representadas no gráfico;
  2. Sabemos que o peso do saco de batatas, que representamos por \( x \), é unidimensional (\( n = 1 \)) e não varia.
    Temos um sistema dinâmico contínuo, discretizado em 50 passos, com \( \delta_t = 1 \) e equações: \[ \SYS{ \dot{x} &=& 0\\ z &=& x + \nu } \]
    Portanto,

  3. O peso foi directamente observado, com ruído. Portanto:

  4. O estado inicial da estimativa é \( x_0,P_0 = 0,\mx{c}{1} \) (fazendo \( P_0 \not= 0 \) permitimos que a nossa estimativa inicial esteja errada e que o filtro eventualmente convirja);

Fazendo os cálculos do FK obtemos, no fim, \( \hat{x}_{50} = 22.1305 \) e os valores intermédios ficam ilustrados no gráfico seguinte:

plot of chunk unnamed-chunk-2

Claro, usar FK para estimar um processo constante é um exemplo de “excesso de força”. O gráfico seguinte compara o FK com duas outras estimativas muito mais simples: a média acumulada e a média flutuante.

plot of chunk unnamed-chunk-3

É visível, neste último gráfico, que a estimativa feita pelo FK não se destingue significativamente da média acumulada. Já a estimativa dada pela média flutuante reflete a variação do ruído.

Finalmente o valor real do peso do saco de batatas é revelado \( x = 22.1234 \textrm{Kg} \):

plot of chunk unnamed-chunk-4

A média acumuldada e FK

plot of chunk unnamed-chunk-5

Um olhar atento aos gráficos acima revela que as flutuações de FK são (quase) exactamente as mesmas que a média acumulada. De facto, os valores só diferem porque os valores iniciais são diferentes. Conforme são feitas mais observações esse valor inicial vai perdendo peso na estimativa e esta depende cada vez mais dos valores observados.

Vejamos as contas para a média acumulada. Supondo que após a \( n \)-ésima observação a média acumulada é \( a \). Na \( n+1 \)-ésima observação é medido \( z \). Então a actualização da média acumulada é:

\[ a \gets \frac{an + z}{n+1} \]

Ora, fazendo \( e = z - a \) vem \( z = e + a \) e na fórmula acima obtemos \[ a \gets \frac{an + e + a}{n+1} = a + \frac{1}{n+1}e \]

Para FK, começemos pelo princípio, pela primeira iteração.

No início do processo começamos por “ignorar completamente” a localização da constante. Essa ignorância exprime-se fazendo \( P_0 \to +\infty \) e \( \hat{x}_0 \) qualquer.

No passo de previsão temos:

\[ \SYS{ \hat{x}_1 &\gets& A \hat{x}_0 + B u = \mx{1}{1}\hat{x}_0 + \mx{1}{0}0 = \hat{x}_1\\ P_1 &\gets& A P_0 A^{\top}+Q = \mx{1}{1} P_0 \mx{1}{1}^{\top} + \mx{1}{\varepsilon} \to +\infty } \]

Segue-se o passo de correcção: \[ \SYS{ e_1 &=& z_0 - C \hat{x}_1 = z_0 - \hat{x}_1\\ S_1 &=& C P_0 C^{\top} + R = P_0 + R\\ K_1 &=& P_0 C^{\top} S_1^{-1} = \frac{P_0}{P_0 + R} \stackrel{P_0 \to +\infty}{\to} 1 } \] e corrigir as estimativas do estado e da covariância: \[ \SYS{ \hat{x}_1 &\gets& \hat{x}_1 + K_1e_1 = \hat{x}_0 + \frac{1}{1}e_1\\ P_1 &\gets& \AT{\mathrm{I} - K_1 C} P_0 = \AT{1 - \frac{P_0}{P_0 + R}} P_0 \stackrel{P_0 \to +\infty}{\to} R } \]

Nestas contas surge a actualização da covariância da estimativa: \[ P' = \AT{1 - \frac{P}{P+R}}P; P_1 = R \] Iterando, obtemos: \[ P_{n} = \frac{1}{n}R \] e substituindo no cálculo de \( K \): \[ K_{n} = \frac{P_n}{P_n + R} = \frac{\frac{1}{n}R}{\frac{1}{n}R + R} = \frac{1}{n\AT{\frac{1}{n} + 1}} =\frac{1}{n+1} \]

Ou seja, a actualização da \( n \)-ésima estimativa é: \[ \hat{x} \gets \hat{x} + \frac{1}{n+1}e \] tal como na actualização da média acumulada.

Um caso menos simples: Acompanhar o movimento de uma partícula acelerada

De seguida descreve-se um modelo dinâmico, parcialmente controlável e parcialmente observável, suficientemente simples para ser acompanhado passo-a-passo mas suficientemente completo para ilustrar os aspectos principais dos FK.

Consideremos uma partícula que se desloca ao longo de uma linha vertical e representamos por \( p \) a posição da partícula. Supomos que podemos controlar directamente a aceleração e obtemos \( \ddot{p} = u \). O facto de termos uma segunda derivada sugere que devemos considerar um processo com duas dimensões, posição e velocidade: \[ x = \mx{c}{p \\ \dot{p}} \]

A forma LIT do processo é dada por \[ \dot{x} = \mx{cc}{0 & 1\\0 & 0}x + \mx{c}{0\\ 1}u \] e supondo que podemos observar apenas a posição, temos \[ z = \mx{cc}{1 & 0}x. \]

Resumindo \[ \SYS{ x &=& \mx{c}{p \\ \dot{p}}\\ u &=& \ddot{p}\\ A &=& \mx{cc}{0 & 1\\0 & 0}\\ B &=& \mx{c}{0\\ 1}\\ C &=& \mx{cc}{1 & 0} } \]

Sobre o ruído, vamos supor que o ruído do processo é bem menor do que das observações. Em particular, \[ \SYS{ Q &=& \mx{cc}{10^{-4} & 0 \\ 0 & 10^{-4}} \\ R &=& \mx{c}{5\times10^{-2}} } \]

Finalmente, supomos que a partícula se desloca durante \( 15\textrm{s} \) e, nesse intervalo de tempo é observada e controlada 100 vezes (uma vez cada \( 0.15\textrm{s} \)). Portanto temos

Para o cálculo do FK é necessário conhecer o sinal de controlo, representado no gráfico seguinte, que mostra uma fase inicial de aceleração positiva, uma fase média de movimento uniforme e uma fase final de desaceleração:

plot of chunk unnamed-chunk-6

As observações que resultam (por simulação) daquele sinal de controlo ficam ilustradas no gráfico seguinte, em que, de facto, se pode observar que a posição da partícula inicialmente aumenta e depois diminui.

plot of chunk unnamed-chunk-7

Dadas estas observações, começemos por observar, desde já, que a média acumulada e a média flutuante são métodos demasiado simplistas para este problema:

plot of chunk unnamed-chunk-8

Neste gráfico é visível que a estimativa pela média acumulada é um desastre completo enquanto que a média flutuante tem quase tanto ruído quanto as observações. Mas estas estimativas foram feitas ignorando por completo o que sabemos sobre o funcionamento do processo. Isto é, nestas estimativas não é usado o modelo \( \dot{x} = Ax + Bu + \omega; z = Cx + \nu \).

Fazendo os cálculos do FK obtemos uma linha que, após a fase inicial, acompanha a zona central da “mancha das observações”. Para efeitos de referência, o valor real (simulado) do processo também é mostrado.

plot of chunk unnamed-chunk-9

Conclusões

O processo de estimação por Filtros de Kalman foi apresentado de forma a previligiar o entendimento prático. Nessa linha aconselha-se vivamente:

  1. Implementar uma “mini” biblioteca que suporte estas contas, incluindo:
  2. Experimentar, experimentar, experimentar:

Notas

1: Para a posição usamos as letras \( p,q \) em vez de \( x,y \) porque estas últimas têm um significado especial no resto do texto. A pose, em particular, ou o estado do sistema, em geral, são identificados por \( x \).

Agradecimentos

As críticas ferozes e pertinentes do Miguel ajudaram decisivamente a melhorar este texto.

Carnaxide, 10 de Maio de 2013 / fCoelho