TEORIA E PRÁTICA DO FILTRO DE KALMAN

Universidade Estadual do Oeste do Paraná Parque Tecnológico Itaipu Centro de Engenharias e Ciências Exatas Curso de Extensão TEORIA E PRÁTICA DO FILTRO DE KALMAN Prof. Carlos Henrique Farias dos Santos carlosh.cece@gmail.com TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 1

Estrutura da aula 1. Introdução 2. Teoria de Sistemas Lineares TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 2

Justificativa do curso Desde que R. E. Kalman publicou sua idéia, no início dos anos 1960, o Filtro de Kalman (FK) está entre as mais importantes contribuições para a ciência dos sistemas de controle no século XX e tem sido objeto de extensivas pesquisas. O impacto deste trabalho é comparado aos trabalhos de Nyquist, Bode e Wierner, nos anos de 1920, 1930 e 1940, respectivamente. Ele vem sendo aplicado em vários problemas práticos em diferentes campos, particularmente na área de navegação autônoma. O que é o Filtro de Kalman? Teoricamente: O filtro de Kalman é um estimador denomidado problema linear quadrático, o qual consiste no problema de estimar o estado instantâneo de um sistema dinâmico linear perturbado por ruído branco, através do uso de medições linearmente relacionadas com este estado, porém corrompidas pelo ruído branco. Definições: Estimação é o processo de inferir o valor de uma quantidade de interesse a partir de observações indiretas, imprecisas e incertas. O propósito da observação pode ser: (a) determinação da órbita de planetas (Laplace, Gauss, Legendre); (b) determinação de posição e velocidade de uma aeronave em um sistema de controle de tráfico aéreo; (c) determinação de parâmetros de um modelo de sistema como forma de predizer o estado físico do mesmo, etc... Filtragem é a estimação do estado corrente de um sistema dinâmico a razão do termo consiste no processo de obter a melhor estimação a partir TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 3

de dados ruidosos. O termo filtragem é assim usado no sentido de eliminar um sinal indesejado, o ruído branco, neste caso. Podemos admitir que o sinal dos sensores do sistema dinâmico tenham um perfil de um sinal randômico com um valor médio zero e uma densidade espectral constante chamada de ruído branco estacionário. Um vetor de ruído branco, possui cada elemento como um ruído branco, e pode ser considerado como o vetor de estados de um sistema estocástico, chamado de processo de ruído branco. Um sistema estocástico é um sistema governado por leis físicas que ainda que suas condições iniciais sejam conhecidas precisamente, é impossível determinar os estados do sistema em um tempo futuro (probabilidade). Praticamente: Uma das primeiras aplicações do filtro de Kalman foi o controle de sistemas dinâmicos complexos tais como: processos de manufatura, aeronáutica e astronáutica, navios, etc... Para controlar um sistema dinâmico, precisa-se primeiro saber o que ele está fazendo. Para estas aplicações, nem sempre é possível ou desejável medir toda variável que se deseja controlar, e o filtro de Kalman permite uma maneira de inferir a informação faltante através de medições indiretas e ruídos. Sob o ponto de vista prático, algumas perspectivas do filtro de Kalman podem ser consideradas neste curso: Trata-se apenas de uma ferramenta: Ele não resolve nenhum problema por si mesmo, por outro lado, ele pode tornar sua solução mais fácil. Não se trata de uma ferramenta física, de um ente ou ferramenta matemática para tornar o trabalho mental mais eficiente. É um programa computacional: Ele foi chamado de idealmente preparado para um computador digital, em parte porque utiliza uma representação finita de um problema de estimação por um número TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 4

fixo de variáveis. Entretanto, admite-se que essas variáveis sejam números reais, com precisão infinita. Trata-se de uma caracterização estatística de um problema de estimação: Ele mais que um estimador, porque ele propaga o estado corrente de conhecimento de um sistema dinâmico, incluindo a influência estatística de perturbações randômicas e os efeitos de todas medições passadas. Objetivo Habilitar o aluno nos fundamentos do trabalho de Kalman como forma de estimular o interesse pela contribuição científica e a exploração de aplicações recentes do tema, tais como: sistemas de navegação autônoma, e detecção, análise e diagnóstico de faltas em sistemas mecânicos e elétricos, fusão de sensores, etc... Conteúdo Programático ETAPA I: Fundamentos Teóricos (7 aulas) A - Introdução; B - Teoria de Sistemas Lineares; C - Teoria da Probabilidade; D - Estimação dos Mínimos Quadrados. ETAPA II: Análise e Projeto do Filtro de Kalman (14 aulas) A - O Filtro de Kalman; B - A Função de Observador de Estados do Filtro de Kalman; TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 5

C - O Ganho do Filtro de Kalman e Equações de Variância; D - Filtro de Kalman em Regime Estacionário; E - Filtros de Ordem Reduzida e Ruídos Correlacionados; F - Controle Estocástico: O Teorema da Separação; G - O Filtro de Kalman Extendido; H - A Informação do Filtro. ETAPA III: Aplicações do Filtro de Kalman (5 aulas) A - Sistemas de Navegação; C - Fusão de Sensores; D - Sistemas de Detecção de Faltas. TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 6

Teoria de Sistemas Lineares Esta seção faz uma revisão de alguns conceitos essenciais da teoria de sistemas lineares. Conceitos como manipulação matricial são necessários para a implementação do filtro de Kalman. Álgebra Matricial; Sistemas Lineares; Sistemas Não-lineares; Discretização; Simulação; Estabilidade; Controlabilidade e Observabilidade TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 7

Álgebra Matricial Um vetor consiste de escalares que são arranjados em linhas ou colunas. Por exemplo, o vetor [ 1 3 π ] Este vetor é chamado de vetor 1 3 porque possui uma linha e três colunas (vetor linha). Enquanto o vetor 2 π 2 j 0 Este vetor é chamado de vetor 4 1 porque possui quatro linhas e uma coluna (vetor coluna). Uma matriz consiste de escalares que são arranjados em um retângulo. Por exemplo, a matriz 2 3 0 π 2 j 0 é uma matriz 3 2 porque possui 3 linhas e 2 colunas. O rank de uma matriz é definido como o número de linhas ou colunas linearmente independentes. O rank de uma matriz A é frequentemente indicado com a notação ρ(a). O rank de uma matriz é sempre menor ou igual ao número de linhas, e também menor ou igual ao número de colunas. Por exemplo, a matriz TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 8

1 2 2 4 tem um rank igual a um porque tem apenas uma linha linearmente independente, pois as duas linhas são múltiplas uma da outra. Além disso, existe apenas uma coluna linearmente independente, as duas colunas são múltiplas uma da outra. Por outro lado, a matriz 1 3 2 4 tem um rank igual a dois porque possui duas linhas linearmente independentes. Ou seja, não existem escalares não nulos c 1 e c 2 capazes de fazer c 1 [ 1 3 ] + c 2 [ 2 4 ] [ = 0 0 ] assim, as duas linhas são linearmente independentes. Além disso, a matriz tem duas colunas linearmente independentes. Ou seja, não existem escalares não nulos c 1 e c 2 capazes de fazer c 1 1 2 + c 2 3 4 = 0 0 portanto, as duas colunas são linearmente independentes. Uma matriz n m cujo rank seja igual a min(n, m) denomina-se de rank completo. A nulidade de uma matriz A n m é calculada por [m ρ(a)]. A transposta de uma matriz (ou vetor) pode ser obtida através da mudança de linhas por colunas e vice-versa. A transposta de uma matriz é indicada por um superescrito T, como em A T. Por exemplo, se A for uma matriz r n, TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 9

A 11 A 1n.. A r1 A rn então A T é uma matriz n r A 11 A r1.. A 1n A rn Álgebra Matricial: A soma (A + B) e a diferença (A B) são definidas apenas se as dimensões de A e B sejam compatíveis. Suponha que A seja uma matriz n r e B seja uma matriz r p. Então o produto de A e B é escrito como C = AB. Cada elemento na matriz produto C é computado como, C ij = r A ij B kj i = 1,..., n j = 1,..., p (1) k=1 A matriz produto AB é definida apenas se o número de colunas em A for igual ao de linhas em B. Vale destacar que a multiplicação de matrizes não é comutativa. Em geral, AB BA. O determinante de uma matriz é definido de forma indutiva para matrizes quadradas. Considere uma matriz A n n. O determinante de A é definido como A = n ( 1) i+j A ij A (i,j) (2) j=1 onde A (i,j) denota matriz formada pelo cancelamento da i-ésima linha e TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 10

j-ésima coluna de A, para qualquer valor de i [1, n]. Esta abordagem recebe a denominação de Expansão de Laplace de A ao longo de sua i-ésima linha. O determinante de A pode também se definido como, A = n ( 1) i+j A ij A (i,j) (3) i=1 para qualquer valor de j [1, n]. Esta abordagem recebe a denominação de Expansão de Laplace de A ao longo de sua j-ésima linha. Algumas propriedades dos determinantes são, AB = A B (4) admitindo que A e B sejam quadradas e tenham a mesma dimensão. Além disso, onde λ i são os autovalores de A. A = n λ i (5) A inversa de uma matriz A é definida como a matriz A 1 tal que AA 1 = A 1 A = I, onde I é a matriz identidade. Uma matriz não pode ter inversa a menos que seja quadrada. Algumas matrizes quadradas não possuem inversa. Uma matriz que não possui uma inversa é chamada de singular. Uma matriz que tem inversa é chamada de não-singular, por exemplo, 1 0 2 3 i=1 1 0 2/3 1/3 = 1 0 0 1 A não-singularidade de uma matriz A n n pode ser estabelecida de várias formas, algumas delas são destacadas a seguir: TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 11

A é não-singular; A 1 existe; O rank de A é igual a n. As linhas de A são linearmente independentes; As colunas de A são linearmente independentes; A = 0; Ax = b tem uma solução única x para todo b; 0 não é um autovalor de A. O traço de uma matriz quadrada é definido como a soma dos seus elementos diagonais: T r(a) = i A ii (6) Algumas propriedades do traço de uma matriz quadrada são: T r(a) = i λ i (7) T r(ab) = T r(ba) (8) Em palavras, o traço de uma matriz quadrada é igual a soma dos seus autovalores. Além disso, o traço de um produto matricial é independente da ordem na qual as matrizes são multiplicadas. A norma dois de um vetor coluna de números reais, também denominado de norma Euclidiana, é definida como segue: TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 12

x 2 = x T x (9) = x 2 1 +... + x 2 n (10) Uma matriz A n n tem n autovalores e n autovetores. O escalar λ é um autovalor de A, e o vetor x n 1 é um autovetor de A, se a seguinte equação for assegurada: Ax = λx (11) Uma matriz simétrica n n A pode ser caracterizada como: Positiva definida se x T Ax > 0 para todo vetor x (n 1) não-nulo. Isto é equivalente a dizer que todos os autovalores de A são números reais positivos. Positiva semidefinida se x T Ax 0 para todo vetor x (n 1). Isto é equivalente a dizer que todos os autovalores de A são números reais não-negativos. Negativa definida se x T Ax < 0 para todo vetor x (n 1) não-nulo. Isto é equivalente a dizer que todos os autovalores de A são números reais negativos. Negativa semidefinida se x T Ax 0 para todo vetor x (n 1). Isto é equivalente a dizer que todos os autovalores de A são números reais negativos. Indefinida se não encaixa em nenhuma das categorias anteriores. Lema da Inversão Matricial: Suponha que tenhamos uma matriz particionada A B onde A e D C D são matrizes quadradas inversíveis, e B e C são matrizes que podem ser ou não quadradas. Definimos as matrizes E e F como segue: TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 13

E = C = D CA 1 B A BD 1 C Admita que E seja inversível. Assim, podemos mostrar que A C B D A 1 + A 1 BE 1 CA 1 E 1 CA 1 A 1 BE 1 E 1 = I + BE 1 CA 1 BE 1 CA 1 CA 1 + CA 1 BE 1 CA 1 DE 1 CA 1 BE 1 + BE 1 CA 1 BE 1 + DE 1 = I 0 CA 1 (D CA 1 B)E 1 CA 1 (D CA 1 B)E 1 = I 0 0 I TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 14

Agora assuma que F seja inversível. Assim, podemos mostrar que = A C B F 1 D D 1 CF 1 = AF 1 BD 1 CF 1 CF 1 CF 1 A 1 BE 1 E 1 BE 1 + BE 1 CA 1 BE 1 + DE 1 (A BD 1 C)F 1 0 0 (D CA 1 B)E 1 = I 0 0 I Portanto, as equações de E e F são duas expressões para a inversa de A B. Desde que são inversas da mesma matriz, elas devem ser C D iguais. Concluímos que a partição superior esquerda das matrizes são iguais, o que fornece, F 1 = A 1 + A 1 BE 1 CA 1 (12) Agora podemos usar as definições de E e F para obter (A BD 1 C) 1 = A 1 + A 1 B(D CA 1 B) 1 CA 1 (13) Esta equação é chamada de lema de inversão de matriz, também referida com outros termos como fórmula de Sherman-Morrison, Identidade de Woodbury e fórmula da matriz modificada. O lema da inversão da matriz é frequentemente estabelecido um pouco diferente mas de forma equivalente. Por exemplo, (A + BD 1 C) 1 = A 1 A 1 B(D CA 1 B) 1 CA 1 (14) TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 15

O lema da inversão de matriz pode algumas vezes ser usado para reduzir o esforço computacional da inversão matricial. Suponha que A seja n n, B seja n p, C seja p n, D seja p p, e p < n. Suponha que saibamos A 1, e queiramos adicionar alguma quantidade a A e então computar a nova inversa. A computação direta da nova inversa seria uma inversão n n. Mas se a nova matriz a ser invertida puder ser escrita no formato da parte esquerda da equação anterior, então podemos usar o lado direito da última equação para computar a nova inversa, e o mesmo requer uma inversão p p no lugar de uma inversão n n. EXEMPLO: Em sua firma de investimento, você percebe que em Janeiro o índice de estoque de câmbio de Nova Iorque diminuiu 2%, o índice de estoque de câmbio Americano aumentou 1%, e a Nasdaq aumentou 2%. Como resultado, investidores aumentaram seus depósitos por 1%. No mês seguinte, os índices mudaram por 4%, 3% e 2%, respectivamente, e os depósitos dos investidores aumentaram 2%. No mês seguinte, os índices mudaram por 5%, 1% e 5% respectivamente, e o depósito dos investidores aumentaram em 2%. Você suspeita que as mudanças de investimento y possam ser modeladas como y = g 1 x 1 + g 2 x 2 + g 3 x 3, onde as variáveis x i são as variáveis de mudança de estoque, e g i são constantes desconhecidas. Para determinar as constantes g i você precisa inverter a matriz O resultado é A = 2 1 2 4 3 2 5 1 5 (15) TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 16

A 1 = 1 6 g = A 1 = 1 6 13 3 4 10 0 4 11 3 2 Isso permite que você use as variações dos índices para prever as mudanças de investimento no mês seguinte, o que lhe permite programar melhor o pessoal e os recursos do computador. No entanto, logo depois de descobrir que a mudança NASDAQ no terceiro mês foi realmente 6% em vez de 5%. Isso significa que, a fim de encontrar as constantes g i você precisa para inverter a matriz A = 1 2 1 1 2 2 2 1 2 4 3 2 5 1 6 (16) Você está cansado de inverter matrizes e gostaria de usar a inversa de A para calcular a inversa de A. Lembrando-se do lema de inversão da matriz, você percebe que A = A + BD 1 C, onde [ B = 0 0 1 ] T TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 17

[ C = 0 0 1 ] D = 1 Você usa o lema da inversão de matrizes para computar (A ) 1 = (A + BD 1 C) 1 = A 1 A 1 B(D CA 1 B) 1 CA 1 O termo (D CA 1 B) 1 que precisa ser invertido é um escalar, assim a inversão fica simples. A 1 = 4.00 1.00 1.00 3.50 0.50 1.00 2.75 0.75 0.50 g = A 1 0 0.5 0.25 Neste exemplo o uso do lema de inversão de matrizes não é tão necessário, visto que se trata de uma matriz 3 3. Entretanto, com matrizes maiores, tais como 1000 1000, o uso do lema de inversão de matrizes é significativo. 1 2 2 TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 18

Sistemas Lineares Um sistema linear determinístico em tempo contínuo pode ser descrito pelas equações: ẋ = Ax + Bu (17) y = onde x é o vetor de estados, u é o vetor de controle, e y é o vetor de saída. A matriz A é frequentemente chamada de matriz do sistema, B é matriz de entradas, e C é a matriz de saída. Se as matrizes A, B e C são constantes então a solução de 17 é dada por, Cx x(t) = e A(t t 0) x(t 0 ) + y(t) = Cx(t) t t 0 e A(t τ) Bu(τ)dτ onde t 0 é o tempo inicial do sistema e considerado 0. No caso de entrada nula, tem-se x(t) = e A(t t 0) x(t 0 ) (18) Por esta razão, e At é chamada de matriz de transição de estados do sistema. É a matriz que descreve como os estados mudam a partir da condição inicial sem a presença de entradas externas. Podemos evoluir a equação anterior em t = t 0 para ser que e A0 = I (19) Entretanto, como poderemos calcular a exponencial da equação 17? TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 19

Existem várias formas diferentes de computar esta quantidade. Três das mais utilizadas são descritas a seguir: e At = (At) j j=0 = L 1 [(si A) 1 ] j = QeÂt Q 1 A primeira expressão não é muito frequente para propósitos computacionais, pois trata-se de uma soma infinita. A segunda expressão usa a transformada inversa de Laplace para computar e At. Na terceira equação, Q é uma matriz cujas colunas comportam os autovalores de A, e  é a forma de Jordan de A. A matriz  é frequentemente diagonal,  =  11 0 0 0  22 0........ eât = 0  nn eâ11 0 0 0 eâ22 0........ 0 eânn TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 20

Sistemas Não-lineares A forma geral de um sistema não-linear em tempo contínuo pode ser escrito como ẋ = f(x, u, w) (20) y = h(x, v) onde f( ) e h( ) são funções de valores arbitrários. Utilizamos w para indicar o ruído do processo e v indica o ruído da medição. Se f( ) e h( ) são funções do tempo t, então o sistema é variante no tempo. De outro modo o sistema é invariante no tempo. Se f(x, u, w) = Ax + Bu + w e h(x, v) = Hx + v, então o sistema é linear. De outro modo o sistema é não-linear. No sentido de aplicar ferramentas da teoria de sistemas lineares para sistemas não-lineares, precisamos linearizar o sistema não-linear. Em outras palavras, precisamos encontrar um sistema linear que seja aproximadamente igual ao sistema não-linear. Para tanto, começamos com um vetor de funções não-lineares f( ) de um escalar x. Nos expandimos f(x) em uma série de Taylor em torno de algum ponto de operação (também chamado de ponto de linearização), x = x, definindo x = x x: f(x) = f( x) + f x x + 1 2 f x x 2 + 1 3 f x x 3 + (21) x 2! x 2 3! x 3 Agora suponha que x seja um vetor 2 1. Isto implica que f(x) seja uma função não-linear de duas variáveis independentes x 1 e x 2. A expansão da série de Taylor de f(x) torna-se f(x) = f( x) + f x 1 x x 1 + f x 2 x x 2 + (22) TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 21

OBRIGADO TEORIA E PRÁTICA DO FILTRO DE KALMAN Aula 01 22