Filtro de Kalman. Teoria e Aplicação para Iniciantes. Prof. Dr. Marco Antonio Leonel Caetano. M&V Consultoria e Treinamento

Transcrição

1 Filtro de Kalman Teoria e Aplicação para Iniciantes Prof. Dr. Marco Antonio Leonel Caetano M&V Consultoria e Treinamento

2 A História da Filtragem de Sinais Filtro de Wiener (contínuo) Filtro de Kolmogorov (discreto) Filtro de Kalman (linear e estendido)

3 Processos Estocásticos Sinal no tempo Estatística (variável aleatória)

4 O que caracteriza um sinal? Média Desvio Padrão ( volatilidade ) Probabilidade Sinal com baia volatilidade Sinal com alta volatilidade

5 Resolvendo Sistema Linear Observar o seguinte sistema linear: 4 5 Para resolver seguem-se duas maneiras diferentes: () Isolar da primeira equação e substituir na segunda. () Transformar o sistema em matrizes e vetores.

6 Primeira Resolução (a) Isolando na primeira equação: (b) Substituindo na segunda equação: (c) Substituindo em isolado em (a): 3 Solução: 3 3

7 Segunda Resolução 4 5 (a) Transformar o sistema em representação matricial: 4 5 A B (b) Resolve-se agora o sistema : AX = B (c) A solução deve envolver a inversa da matriz A, ou seja, A - B A AX A (d) Como A -.A é matriz identidade a solução será: X = A - B (e) No matlab basta : = inv(a)*b

8 No Matlab B tem que ser transposto! Ou B = [ 4 ; ]

9 Problema com dimensão de matrizes Resolver sistema linear com mesmo número de linhas e colunas é fácil usando matlab. Mas quando se tem MAIS equação que incógnita a inversa da matriz não é possível equações incógnitas (,)????

10 Método dos Mínimos Quadrados A solução para o problema anterior é encontrar o vetor mais próimo possível tal que o sistema AX = B seja o mais verdadeiro possível! Deve-se encontrar o vetor cujo erro do sistema seja o menor possível ao quadrado. Por isso o método se chama MÍNIMOS QUADRADOS. Eemplo: reta de regressão linear Reta que mais se aproima dos pontos amostrados Y a angulo X b=tangente(angulo) Os resíduos dessa diferença são os menores possíveis quando elevados ao quadrado.

11 A Estimativa do Método dos Mínimos Quadrados (MMQ) Seja A B T A A A T B T A A T T T. A A. A A. A B identidade Logo, para encontrar o vetor mais próimo possível da solução: T T A A A B

12 Aplicação dos MMQ à medidas Imaginar que duas variáveis foram acompanhadas por 3 dias e tiveram seus valores relacionados na tabela a seguir: medida medida 0,0 3 0,8 0,8 36 Suponha que eista a seguinte relação entre as medidas: Como estimar? medida medida

13 Solução via MMQ 0,0 0,8 0, Sistema com 3 equações e incógnita Neste caso as matrizes A e B serão: 0,0 3 A 0,8 B 0,8 36 Então, lembrando que o nesse problema é o valor de e: Qual a solução? T T A A A B

14 Solução (a) A T A 0, 0,8 0, 0,8 0,8 0,8 0, 08 (b) A T B 0, 0,8 0, 0,8 0,8 0,8 5, 34 Assim, 5,34 0,08 7

15 A Solução Numérica T T A A A B

16 Estimativa de parâmetros - Ajuste de Função Dada uma tabela de dados 3 3 : : y y y Y X Deseja-se encontrar a melhor função linear que ajuste y aos valores de : c c y 0 Observando que os dados são inseridos na função da seguinte forma: ) ( ) ( 0 i c c i y O sistema a ser resolvido é: n y n y y c c 0. A X B

17 Eercício Fazer um programa para entrar com n valores de e de y e no final o programa deve ajustar a função linear pelo método dos mínimos quadrados. Use como eemplo a tabela a seguir: X Y 4 5 Solução

18 Eercício Modificar o programa anterior para fazer o gráfico dos pontos da Tabela e dos pontos da reta ajustada y(i) = c0 + c* (i)

19 Solução pontos da tabela RETA AJUSTADA

20 Estimação Recursiva Considere o problema da estimativa de uma constante escalar não Aleatória baseada em medidas anteriores, corrompidas por ruído: z v i =,,3,..., i i onde vi é ruído branco gaussiano ( média zero e desvio padrão fio). Qual a melhor estimativa para?

21 A média é a melhor estimativa para! i z i Para uma medida adicional (+) a nova estimativa será i z i

22 Deve-se manipular os termos para deiar a estimativa sempre em função das medidas anteriores: ˆ i i z z i z i z z média i z i z

23 Somando-se e subtraindo z K z ˆ ˆ

24 Estimação recursiva de variáveis. z Ganho do Sistema informação ainda não utilizada

25 Eemplo Estimar a constante a=0 com ruído de medida v (média = 0, desvio=5) para 00 medidas de a.

26 Programando a fórmula do mínimos quadrados recursivo:

27 Resultado

28 Para n simulações

29 Comparando com as medidas

30 Por que usar a distribuição gaussina? Segundo o teorema do limite central, todas convergem para a normal para um número grande de pontos Teorema do Limite Central NORMALIZAÇÃO

31 Para uma única variável Média: Variância: f ( ) e ( )

32 Para duas variáveis Distribuição Espacial f (, y) e ( ) ( y y) ( y y ) ( ) ( y y) Média de : Média de y y Desvio Padrão de : Desvio padrão de y: Covariância de e y: Correlação de e y: y y

33 Mas a Covariância é uma matriz ( P ) y y y y P A função gaussina corrigida para duas variáveis / ) ( ) ( ) ( ) ( ), ( P e y f y y P y y onde P é o determinante da matriz de covariância É necessário uma correção na fórmula pois,

34 Problema E se as variáveis forem vetores de dados com sinais do tipo: n A covariância seria: n n n P Impraticável! Todos os sinais teriam que serem armazenados em gigantescos bancos de dados para o calculo no fim. Solução: Filtro de Kalman

35 Como o filtro de Kalman estima variáveis? f,,, n n n tempo sinal

36 Algoritmo Esquemático Trabalha com dados Equação de Propagação Equação de Atualização Equação de Covariância Satélite dados Bóia Equação de Atualização ˆ Equação de Atualização ˆ Equação de Propagação + ˆ Equação de Propagação ˆ + Equação de Propagação ˆ 3 tempo

37 Algumas Definições Variável de Estado () - Representa a variável de estudo do modelo matemático para a previsão dos dados futuros. E: temperatura, salinidade. - O filtro linear usa modelo linear. - O futuro é o presente com alguma correção e corrompido por ruído w. A C Variável de Medida (z) - Variável que simula a aquisição dos dados de um sensor. - Para o filtro linear o modelo de medida é linear com ruído v do sensor. z H Propagação ( do estado ou da covariância ) - É a simulação do que se espera de dados futuros baseados em dados. - Utiliza um modelo para a previsão. Atualização ( do estado ou da covariância ) - É a correção da previsão usando os novos dados coletados pelo sensor.

38 Assumir. O estado é corrompido por perturbação do tipo ruído branco gaussinano com média zero e covariância Q (matriz para todos os valores propagados e atualizados).. A medida do sensor é supostamente corrompida por ruído branco gaussiano com média zero e covariância R n Q nz z z R

39 Propagações Propagação do Estado (à priori) Qual a melhor estimativa? A média! As barras representam a média. A C A ˆ ˆ é o rúido branco do estado ˆ Propagação da covariância do estado (à priori) T T T T CQC A AP P C A C A P P

40 Atualizações Atualização do estado - A melhor estimativa de quando se tem medida z é aquela que minimiza o termo do epoente da distribuição gaussiana. / ) ( ) ( ) ( ) ( ), ( P e y f y y P y y Isso é desejado! Colocando a medida na distribuição gaussiana: Deseja-se minimizar Ou,

41 Isso leva à seguinte relação Isolando o estado para sua estimativa Usando a mesma idéia do mínimos quadrados recursivos para novas medidas chega-se a seguinte equação de atualização ˆ posteriori ˆ priori P priori R H T z Hˆ priori

42 Atualização da covariância A melhor covariância P quando se tem medida z é aquela que zera o erro entre medida e variável: Cuja medida estatística está na matriz de covariância (a barra significa média): ˆ posteriori A melhor estimativa é conseguida substituindo a relação de atualização do estado na equação acima. A relação linear que surge é: P posteriori P priori P HP priori priori H T H A matriz R é a inversa da matriz de covariância do erro do sensor de medida. T R

43 O Filtro de Kalman Priori = ( - ) Posteriori = ( + ) Equação de Propagação: A. propagação do estado T P A. P. A C. Q. C T propagação da covariância do estado Equação de Atualização: T T K P. H. H. P. H R P P K. H. P K. z H. ganho do filtro atualização da matriz de covariância do estado atualização do estado

44 Quando o Filtro Falha. Modelamento errado do estado - Se a matriz de perturbação Q do estado for mal dimensionada em relação ao problema real o que acontece? Modelo Medida R H HP H P K T T T T CQC A AP P K P Q ˆ ˆ H y K ˆ ˆ ˆ H y AK A ˆ ˆ ˆ

45 y Resultado Sinal estimado Errado! Sinal real

46 . Se o ruído de medida é muito menor que a perturbação no estado, ou seja, se R<<<Q. Significa que o sensor é totalmente confiável, pois: R K O filtro não filtra, acompanha as medidas de perto, pois são confiávies!

47 3. Se o ruído de estado é muito menor que o ruído de medida, ou seja, se Q<<<R. Significa que o sensor não é confiável, pois: Q K O filtro só usa o modelo e filtra tudo. Abandona as Medidas, pois K0!

48 4. Se a matriz de covariância da perturbação do estado inicial P(0) for muito baia. Ela significa a memória do filtro. Se P(0) é baia, o filtro não tem memória e demora a energar acontecimentos futuros. P(0) muito baia demora para aprender P(0) ideal aprende rápido

49 Característica ideal da covariância do estado P P(0) alto tempo

50 Covariânica do erro de localização horizontal (Taemasa, Shozo, 007, pag 3849) ERRO NO MODELO

51 O problema da estimativa da temperatura SIMULAÇÃO Dados de temperaturas na localidade dos sensores (satélites, PCD,etc) Dados com ruído. O que se desejava? - É possível estimar os dados em tempo real? - É possível fazer uma previsão confiável com boa antecedência? Solução: Filtro de Kalman

52 Sensores radiosondas (Taemasa, 00) Modelos observado

53 Modelo Utilizado A C. Fez se uma hipótese que A =.. Fez-se a suposição que C =. 3. O valor de H = pois só tem um sensor de temperatura. 4. A perturbação aleatória no estado (temperatura) foi Q= 0,5 o C. 5. O ruído no sensor foi suposto de 0 o C, ou variância R= A matriz de covariância inicial da perturbação do estado P(0)= Finalmente, foi dito ao filtro que a temperatura inicial foi (0)=0 o C, para obrigar a ter uma alta imprecisão e fazer um aprendizado rápido. 8. O tempo final de simulação para aprendizado e filtragem foi de t = 00.

54 Filtro Equação de Propagação: ˆ ˆ P 0,5 P (0)=0 P(0)=40 Equação de Atualização: K P P K ˆ ˆ P 00 K P. z ˆ