MP-208: Filtragem Ótima com Aplicações Aeroespaciais Capítulo 5: Aspectos Computacionais do Filtro de Kalman Davi Antônio dos Santos Departamento de Mecatrônica Instituto Tecnológico de Aeronáutica davists@ita.br São José dos Campos, Brasil Setembro de 2016 1 / 18
Sumário 1 Motivação 2 Filtro Informação 3 Filtro com Atualização Sequencial 4 Filtro Raiz Quadrada 2 / 18
Motivação Dificultades Numétricas do Filtro de Kalman Convencional: 1 Predição: A fórmula de predição da covariância condicional P k+1 k = A k P k k A T k + G k Q k G T k pode resultar numa matriz, embora positiva, não simétrica. Essa dificuldade pode ser remediada por adequada implementação dos produtos de três matrizes ou por uma formulação raiz quadrada. 3 / 18
Motivação 2 Atualização: A fórmula de atualização da covariância condicional P k+1 k+1 = P k+1 k K k+1 C k+1 P k+1 k pode resultar numa matriz não simétrica e/ou indefinida. Essa dificuldade pode ser remediada pelo uso da fórmula de Joseph: P k+1 k+1 = (I K k+1 C k+1 )P k+1 k (I K k+1 C k+1 ) T +K k+1 R k+1 K T k+1 ou por uma formulação raiz quadrada. 4 / 18
Motivação 3 Ganho de Kalman: A fórmula do ganho de Kalman K k+1 = P k+1 k C T k+1 (C k+1 P k+1 k C T k+1 + R k+1 ) 1 tem elevado custo computacional, devido à inversão de matriz, quando a dimensão do vetor de medidas é grande. Essa dificuldade pode ser remediada pelo uso do filtro informação, pelo uso do filtro raiz quadrada, ou por atualização sequencial de medidas escalares. 5 / 18
Filtro Informação Definições Preliminares: Defina, respectivamente, a matriz informação atualizada e a matriz informação predita: L k k (P k k ) 1 (1) L k+1 k (P k+1 k ) 1 (2) Defina também as estimativas transformadas atualizada e predita, respectivamente: ẑ k k L k kˆx k k (3) ẑ k+1 k L k+1 kˆx k+1 k (4) 6 / 18
Filtro Informação Predição: Dados L k k e ẑ k k, pode-se calcular L k+1 k e ẑ k+1 k por: ẑ k+1 k = Π k = A T k L k k A 1 k (5) ( ) 1 K k = Π k G k G T k Π k G k + Q 1 (6) ( ) I K kg T k L k+1 k = ( k ẑ k k + I K kg T k ( I K kg T k A T k ) Π k B k u k (7) ) Π k (8) 7 / 18
Filtro Informação Atualização: Dados L k+1 k e ẑ k+1 k, pode-se calcular L k+1 k+1 e ẑ k+1 k+1 por: ẑ k+1 k+1 = ẑ k+1 k + C T k+1r 1 k+1 y k+1 (9) L k+1 k+1 = L k+1 k + C T k+1r 1 k+1 C k+1 (10) Quando necessário, pode-se recuperar a estimativa filtrada e a correspondente covariância por: P k+1 k+1 = L 1 k+1 k+1 ˆx k+1 k+1 = P k+1 k+1 ẑ k+1 k+1 8 / 18
Filtro com Atualização Sequencial Definição do Problema: Deseja-se agora atualizar a estimativa a priori ˆx k+1 k R n assimilando os componentes escalares de y k+1 R m, um a um sequencialmente. y k+1 [ y k+1,1 y k+1,2... y k+1,m ] T 9 / 18
Filtro com Atualização Sequencial Solução do Problema (Ruídos de Medida Descorrelacionados): Inicialmente, note que a VA referente à i-ésima medida escalar Y k+1,i do instante k + 1 pode, como no capítulo 3, ser modelada por: Y k+1,i = C k+1,i X k+1 + V k+1,i (11) onde C k+1,i R 1 n é uma matriz conhecida, X k+1 R n é o vetor de estados, com distribuição a priori dadas as medidas Y 1:k, Y k+1,1,..., Y k+1,i 1 X k+1 Y 1:k, Y k+1,1,..., Y k+1,i 1 N (ˆx k+1 k+1,i 1, P k+1 k+1,i 1 ) (12) V k+1,i R é o ruído de medida, com distribuição V k+1,i N (0, R k+1,i ), R k+1,i R conhecido e o conjunto { X k+1, V k+1,1,..., V k+1,i } é descorrelacionado. 10 / 18
Filtro com Atualização Sequencial...Solução do Problema: A assimilação MMSE da realização y k+1,i de Y k+1,i à estimativa a priori ˆx k+1 k+1,i 1 é dada por ) K k+1,i = P k+1 k+1,i 1 C T k+1,i/ (C k+1,i P k+1 k+1,i 1 C T k+1,i + R k+1,i ˆx k+1 k+1,i = ˆx k+1 k+1,i 1 + K k+1,i ( y k+1,i C k+1,iˆx k+1 k+1,i 1 ) (13) (14) P k+1 k+1,i = P k+1 k+1,i 1 K k+1,i C k+1,i P k+1 k+1,i 1 (15) 11 / 18
Filtro com Atualização Sequencial Início do Loop: Quando i = 1 a informação a priori da equação (12) fica e, portanto, Fim do Loop: X k+1 Y 1:k N (ˆx k+1 k, P k+1 k ) (16) ˆx k+1 k+1,0 = ˆx k+1 k (17) P k+1 k+1,0 = P k+1 k (18) Após a assimilação da m-ésima medida escalar, obtém-se a estimativa filtrada e a correspondente covariância condicional como sendo: ˆx k+1 k+1 = ˆx k+1 k+1,m (19) P k+1 k+1 = P k+1 k+1,m (20) 12 / 18
Filtro com Atualização Sequencial Ruídos de Medida Descorrelacionados: Considere agora que a covariância R k+1 R m m do ruído de medida vetorial V k+1 R m contenha elementos não nulos fora da diagonal principal. Represente o fator de Cholesky de R k+1 por R 1/2 k+1. Pode-se obter um modelo de medida vetorial transformado: Ȳ k+1 = C k+1 X k+1 + V k+1 (21) com Ȳ k+1 = R 1/2 k+1 Y k+1, C k+1 = R 1/2 k+1 C k+1 e V k+1 = R 1/2 k+1 V k+1, tal que: R k+1 E ( V k+1 V k+1) T = I m (22) Neste caso, utiliza-se os componentes do vetor de medidas transformado Ȳ k+1 nas iterações (13)-(15) da atualização sequencial. 13 / 18
Filtro Raiz Quadrada Definições Preliminares: Sejam as decomposições de Cholesky de P k+1 k e P k+1 k+1 : P k+1 k = S k+1 k S T k+1 k (23) P k+1 k+1 = S k+1 k+1 S T k+1 k+1 (24) em que os fatores de Cholesky S k+1 k R n n e S k+1 k+1 R n n são matrizes triangulares inferiores. Denote ainda aos fatores de Cholesky de Q k e R k por Q 1/2 k e R 1/2 k, respectivamente. 14 / 18
Filtro Raiz Quadrada Predição: Seja a fórmula de propagação temporal da covariância condicional do estado: P k+1 k = A k P k k A T k + G k Q k G T k (25) Usando as definições do slide 14, pode-se imediatamente reescrever (25) na forma S k+1 k S T k+1 k = MMT (26) [ ] M A k S k k G k Q 1/2 k (27) Pode-se então obter S k+1 k = R T, onde R é a matriz triangular superior obtida da decomposição QR de M T. 15 / 18
Filtro Raiz Quadrada Atualização: Seja a fórmula de Joseph para atualização da covariância condicional do estado: P k+1 k+1 = (I K k+1 C k+1 )P k+1 k (I K k+1 C k+1 ) T + K k+1 R k+1 K T k+1 (28) Usando as definições do slide 14, pode-se imediatamente reescrever (28) na forma S k+1 k+1 S T k+1 k+1 = M M T (29) M [ ] (I K k+1 C k+1 )S k+1 k K k+1 R 1/2 k+1 (30) Pode-se então obter S k+1 k+1 = R T, onde R é a matriz triangular superior obtida da decomposição QR de M T. 16 / 18
Filtro Raiz Quadrada Ganho de Kalman: Seja a fórmula do ganho de Kalman: onde K k+1 = P XY k+1 k ( P Y k+1 k) 1 (31) P Y k+1 k = C k+1p k+1 k C T k+1 + R k+1 P XY k+1 k = P k+1 kc T k+1 Seja a decomposição de Cholesky de P Y k+1 k = MMT. Pode-se obter K k+1 resolvendo-se o sistema de equações K k+1 MM T = P XY k+1 k (32) por back- e forward-substitution, respectivamente. 17 / 18
Referências Bar-Shalom, Y., Li, R. X., Kirubarajan, T. Estimation with Applications to Tracking and Navigation. New Jersey: Jonh Wiley & Sons, 2001. Anderson, B. D. O., Moore, J. B. Optimal Filtering. New York: Dover, 2005. Bierman, G. J. Factorization Methods for Discrete Sequential Estimation. New York: Academic Press, 1977. 18 / 18