TEORIA E PRÁTICA DO FILTRO DE KALMAN

(1)

1.

Universidade Estadual do Oeste do Paran´a Parque Tecnol´ogico Itaipu

Centro de Engenharias e Ciˆencias Exatas Curso de Extens˜ao

TEORIA E PR ´

ATICA DO FILTRO DE

KALMAN

Prof. Carlos Henrique Farias dos Santos [email protected]

(2)

2.

Estrutura da aula

1. Introdu¸c˜ao

(3)

3.

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¸cõ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¸cão autônoma.

O que ´

e o Filtro de Kalman ?

Teoricamente:

O filtro de Kalman ´e 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¸cões linearmente relacionadas com este estado, porém

corrompidas pelo ru´ıdo branco. D efini¸c˜oes:

Estima¸cão ´e o processo de inferir o valor de uma quantidade de interesse a partir de observa¸cões indiretas, imprecisas e incertas. O propósito da observa¸cão pode ser: (a) determina¸cão da órbita de planetas (Laplace, Gauss, Legendre); (b) determina¸cão de posi¸cão e velocidade de uma

aeronave em um sistema de controle de tráfico aéreo; (c) determina¸cão de parâmetros de um modelo de sistema como forma de predizer o estado f´ısico do mesmo, etc...

Filtragem ´e a estima¸cão do estado corrente de um sistema dinâmico a razão do termo consiste no processo de obter a melhor estima¸cão a partir

(4)

4.

de dados ruidosos. O termo filtragem ´e 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´ario. 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´astico, chamado de processo de ru´ıdo branco.

Um sistema estocástico ´e um sistema governado por leis f´ısicas que ainda que suas condi¸cões iniciais sejam conhecidas precisamente, é imposs´ıvel determinar os estados do sistema em um tempo futuro (probabilidade). Praticamente:

Uma das primeiras aplica¸cõ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¸cõ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¸cão faltante através de medi¸cões

indiretas e ru´ıdos.

Sob o ponto de vista pr´atico, algumas perspectivas do filtro de Kalman podem ser consideradas neste curso:

• Trata-se apenas de uma ferramenta: Ele n˜ao resolve nenhum

problema por si mesmo, por outro lado, ele pode tornar sua solu¸cã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¸cão finita de um problema de estima¸cão por um número

(5)

5.

fixo de variáveis. Entretanto, admite-se que essas variáveis sejam números reais, com precisão infinita.

• Trata-se de uma caracteriza¸c˜ao estat´ıstica de um problema de

estima¸c˜ao: Ele mais que um estimador, porque ele propaga o estado corrente de conhecimento de um sistema dinˆamico, incluindo a

influência estat´ıstica de perturba¸cões randômicas e os efeitos de todas medi¸cões passadas.

Objetivo

Habilitar o aluno nos fundamentos do trabalho de Kalman como forma de estimular o interesse pela contribui¸c˜ao cient´ıfica e a explora¸c˜ao de

aplica¸cões recentes do tema, tais como: sistemas de navega¸cão autônoma, e deteçcão, análise e diagnóstico de faltas em sistemas mecânicos e

el´etricos, fus˜ao de sensores, etc...

Conte´

udo Program´

atico

ETAPA I: Fundamentos Te´oricos (7 aulas)

A - Introdu¸c˜ao;

B - Teoria de Sistemas Lineares; C - Teoria da Probabilidade;

D - Estima¸c˜ao dos M´ınimos Quadrados.

ETAPA II: An´alise e Projeto do Filtro de Kalman (14 aulas)

A - O Filtro de Kalman;

(6)

6.

C - O Ganho do Filtro de Kalman e Equa¸c˜oes de Variˆancia;

D - Filtro de Kalman em Regime Estacion´ario;

E - Filtros de Ordem Reduzida e Ru´ıdos Correlacionados; F - Controle Estoc´astico: O Teorema da Separa¸c˜ao;

G - O Filtro de Kalman Extendido; H - A Informa¸c˜ao do Filtro.

ETAPA III: Aplica¸c˜oes do Filtro de Kalman (5 aulas)

A - Sistemas de Navega¸c˜ao;

C - Fus˜ao de Sensores;

(7)

7.

Teoria de Sistemas Lineares

Esta se¸cão faz uma revisão de alguns conceitos essenciais da teoria de sistemas lineares. Conceitos como manipula¸cão matricial são necessários para a implementa¸cão do filtro de Kalman.

• Álgebra Matricial; • Sistemas Lineares; • Sistemas Não-lineares; • Discretiza¸cão; • Simula¸cão; • Estabilidade; • Controlabilidade e Observabilidade

(8)

8.

´

Algebra Matricial

Um vetor consiste de escalares que s˜ao arranjados em linhas ou colunas. Por exemplo, o vetor

[

1 3 π ]

Este vetor ´e chamado de vetor 1× 3 porque possui uma linha e trˆes colunas (vetor linha). Enquanto o vetor

       −2 π2 j 0       

Este vetor ´e chamado de vetor 4× 1 porque possui quatro linhas e uma coluna (vetor coluna). Uma matriz consiste de escalares que s˜ao

arranjados em um retˆangulo. Por exemplo, a matriz     −2 3 0 π2 j 0    

´e 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 ´e frequentemente indicado com a nota¸c˜ao ρ(A). O rank de uma matriz ´e sempre menor ou igual ao número de linhas, e também menor ou igual ao número de

(9)

9.   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˜ao existem escalares n˜ao nulos c1 e c2 capazes de fazer c1 [ 1 3 ] + c2 [ 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˜ao nulos c1 e c2 capazes de fazer

c1   1 2   + c2   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¸ca de linhas por colunas e vice-versa. A transposta de uma matriz é indicada por um superescrito T , como em AT. Por exemplo, se A for uma matriz r× n,

(10)

10.     A11 · · · A1n .. . ... Ar1 · · · Arn     ent˜ao AT _´_{e uma matriz n}× r

    A11 · · · Ar1 .. . ... A1n · · · Arn     ´ Algebra Matricial:

A soma (A + B) e a diferen¸ca (A− B) s˜ao definidas apenas se as

dimens˜oes de A e B sejam compat´ıveis. Suponha que A seja uma matriz n× r e B seja uma matriz r × p. Ent˜ao o produto de A e B ´e escrito como C = AB. Cada elemento na matriz produto C ´e computado como,

Cij = r

∑

k=1

AijBkj i = 1, . . . , n j = 1, . . . , p (1)

A matriz produto AB ´e definida apenas se o n´umero de colunas em A for igual ao de linhas em B. Vale destacar que a multiplica¸c˜ao de matrizes n˜ao ´e comutativa. Em geral, AB ̸= BA.

O determinante de uma matriz ´e definido de forma indutiva para

matrizes quadradas. Considere uma matriz A n× n. O determinante de A ´e definido como |A| = n ∑ j=1 (−1)i+jAij|A(i,j)| (2)

(11)

11.

j-´esima coluna de A, para qualquer valor de i ∈ [1, n]. Esta abordagem recebe a denomina¸c˜ao de Expans˜ao de Laplace de A ao longo de sua i-´esima linha. O determinante de A pode tamb´em se definido como,

|A| =

n

∑

i=1

(−1)i+jAij|A(i,j)| (3)

para qualquer valor de j ∈ [1, n]. Esta abordagem recebe a denomina¸c˜ao de Expans˜ao de Laplace de A ao longo de sua j-´esima linha.

Algumas propriedades dos determinantes s˜ao,

|AB| = |A||B| (4)

admitindo que A e B sejam quadradas e tenham a mesma dimens˜ao. Al´em disso, |A| = n ∏ i=1 λ_i (5) onde λi s˜ao os autovalores de A.

A inversa de uma matriz A ´e definida como a matriz A−1 tal que

AA−1 = A−1A = I, onde I ´e 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     1 0 −2/3 1/3   =   1 0 0 1  

A n˜ao-singularidade de uma matriz A n× n pode ser estabelecida de v´arias formas, algumas delas s˜ao destacadas a seguir:

(12)

12.

• A ´e n˜ao-singular; • A−1 _existe;

• O rank de A ´e igual a n.

• As linhas de A s˜ao linearmente independentes; • As colunas de A s˜ao linearmente independentes; • |A| ̸= 0;

• Ax = b tem uma solu¸cão única x para todo b; • 0 não é um autovalor de A.

O tra¸co de uma matriz quadrada ´e definido como a soma dos seus elementos diagonais:

T r(A) = ∑

i

Aii (6)

Algumas propriedades do tra¸co de uma matriz quadrada s˜ao:

T r(A) = ∑

i

λi (7)

T r(AB) = T r(BA) (8)

Em palavras, o tra¸co de uma matriz quadrada é igual a soma dos seus autovalores. Além disso, o tra¸co 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:

(13)

13. ∥x∥2 = √ xT_x ₍₉₎ = √ x2₁+ . . . + x2 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¸cão for assegurada:

Ax = λx (11)

Uma matriz sim´etrica n× n A pode ser caracterizada como:

• Positiva definida se xT_{Ax > 0 para todo vetor x (n}_{× 1) n˜ao-nulo.}

Isto ´e equivalente a dizer que todos os autovalores de A s˜ao n´umeros reais positivos.

• Positiva semidefinida se xT_Ax _{≥ 0 para todo vetor x (n × 1) . Isto ´e}

equivalente a dizer que todos os autovalores de A s˜ao n´umeros reais n˜ao-negativos.

• Negativa definida se xT_{Ax < 0 para todo vetor x (n}× 1) n˜ao-nulo.

Isto ´e equivalente a dizer que todos os autovalores de A s˜ao n´umeros reais negativos.

• Negativa semidefinida se xT_Ax≤ 0 para todo vetor x (n × 1) . Isto ´e

equivalente a dizer que todos os autovalores de A s˜ao n´umeros reais negativos.

• Indefinida se n˜ao encaixa em nenhuma das categorias anteriores.

Lema da Invers˜ao Matricial:

Suponha que tenhamos uma matriz particionada 

 A B

C D



 onde A e D s˜ao matrizes quadradas invers´ıveis, e B e C s˜ao matrizes que podem ser ou n˜ao quadradas. Definimos as matrizes E e F como segue:

(14)

14.

E = D− CA−1B C = A− BD−1C

Admita que E seja invers´ıvel. Assim, podemos mostrar que   A B C D     A−1 + A−1BE−1CA−1 −A−1BE−1 −E−1_CA−1 _E−1   =   I + BE−1CA−1 − BE−1CA−1 −BE−1 + BE−1 CA−1 + CA−1BE−1CA−1 − DE−1CA−1 −CA−1BE−1 + DE−1   =   I 0 CA−1 − (D − CA−1B)E−1CA−1 (D− CA−1B)E−1   =   I 0 0 I  

(15)

15.

Agora assuma que F seja invers´ıvel. Assim, podemos mostrar que   A B C D     F−1 −A−1BE−1 −D−1_CF−1 _E−1   =   AF−1 − BD−1CF−1 −BE−1 + BE−1 CF−1 − CF−1 −CA−1BE−1 + DE−1   =   (A− BD−1C)F−1 0 0 (D− CA−1B)E−1   =   I 0 0 I   Portanto, as equa¸c˜oes de E e F s˜ao duas express˜oes para a inversa de 

 A B

C D



. Desde que são inversas da mesma matriz, elas devem ser iguais. Conclu´ımos que a parti¸cão superior esquerda das matrizes são iguais, o que fornece,

F−1 = A−1 + A−1BE−1CA−1 (12) Agora podemos usar as defini¸c˜oes de E e F para obter

(A− BD−1C)−1 = A−1 + A−1B(D− CA−1B)−1CA−1 (13) Esta equa¸cã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,

(16)

16.

O lema da inversão de matriz pode algumas vezes ser usado para reduzir o esfor¸co computacional da invers˜ao 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˜ao computar a nova inversa. A computa¸cã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¸cão anterior, então podemos usar o lado direito da última equa¸cão para computar a nova inversa, e o mesmo requer uma invers˜ao 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¸cas de investimento y possam ser modeladas como y = g1x1+ g2x2+ g3x3, onde as vari´aveis xi são as vari´aveis de mudan¸ca de estoque, e gi são constantes

desconhecidas. Para determinar as constantes g_i vocˆe precisa inverter a matriz A =     −2 1 2 −4 3 2 −5 1 5     (15) O resultado ´e

(17)

17. A−1 = 1 6     13 −3 −4 10 0 −4 11 −3 −2     g = A−1     1 2 2     = 1 6     −1 2 1    

Isso permite que vocˆe use as varia¸c˜oes dos ´ındices para prever as

mudan¸cas 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¸ca NASDAQ no terceiro mês foi realmente 6% em vez de 5%. Isso significa que, a fim de encontrar as constantes gi você

precisa para inverter a matriz

A′ =     −2 1 2 −4 3 2 −5 1 6     (16)

Vocˆe est´a cansado de inverter matrizes e gostaria de usar a inversa de A para calcular a inversa de A′. Lembrando-se do lema de invers˜ao da matriz, vocˆe percebe que A′ = A + BD−1C, onde

B = [

0 0 1 ]T

(18)

18. C = [ 0 0 1 ] D = 1

Vocˆe usa o lema da invers˜ao de matrizes para computar

(A′)−1 = (A + BD−1C)−1

= A−1 − A−1B(D− CA−1B)−1CA−1

O termo (D− CA−1B)−1 que precisa ser invertido ´e um escalar, assim a invers˜ao 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     1 2 2         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.

(19)

19.

Sistemas Lineares

Um sistema linear determin´ıstico em tempo cont´ınuo pode ser descrito pelas equa¸c˜oes:

˙x = Ax + Bu (17)

y = Cx

onde x é o vetor de estados, u é o vetor de controle, e y ´e o vetor de sa´ıda. A matriz A é frequentemente chamada de matriz do sistema, B ´e matriz de entradas, e C é a matriz de sa´ıda. Se as matrizes A, B e C s˜ao constantes então a solu¸cão de 17 é dada por,

x(t) = eA(t−t0)_x(t 0) + ∫ t t0 eA(t−τ)Bu(τ )dτ y(t) = Cx(t)

onde t0 ´e o tempo inicial do sistema e considerado 0. No caso de entrada nula, tem-se

x(t) = eA(t−t0)_x(t

0) (18)

Por esta raz˜ao, eAt é chamada de matriz de transi¸cão de estados do sistema. É a matriz que descreve como os estados mudam a partir da condi¸cão inicial sem a presen¸ca de entradas externas. Podemos evoluir a equa¸c˜ao anterior em t = t0 para ser que

eA0 = I (19)

(20)

20.

Existem várias formas diferentes de computar esta quantidade. Três das mais utilizadas são descritas a seguir:

eAt = ∞ ∑ j=0 (At)j j = L−1[(sI − A)−1] = QeAtˆ Q−1

A primeira expressão não é muito frequente para propósitos

computacionais, pois trata-se de uma soma infinita. A segunda express˜ao usa a transformada inversa de Laplace para computar eAt. Na terceira equa¸c˜ao, Q ´e uma matriz cujas colunas comportam os autovalores de A, e

ˆ

A ´e a forma de Jordan de A. A matriz ˆA ´e frequentemente diagonal,

ˆ A =        ˆ A₁₁ 0 · · · 0 0 Aˆ22 · · · 0 .. . . .. ... ... 0 · · · ˆAnn        eAtˆ =        eAˆ11 ₀ · · · ₀ 0 eAˆ22 · · · ₀ .. . . .. ... ... 0 · · · eAˆnn       

(21)

21.

Sistemas N˜

ao-lineares

A forma geral de um sistema n˜ao-linear em tempo cont´ınuo pode ser escrito como

˙x = f (x, u, w) (20)

y = h(x, v)

onde f (·) e h(·) são fun¸cões de valores arbitrários. Utilizamos w para indicar o ru´ıdo do processo e v indica o ru´ıdo da medi¸cão. Se f (·) e h(·) são fun¸c˜oes do tempo t, ent˜ao o sistema é variante no tempo. De outro modo o sistema ´e invariante no tempo. Se f (x, u, w) = Ax + Bu + w e h(x, v) = Hx + v, ent˜ao 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¸camos com um vetor de fun¸cões n˜ao-lineares f (·) de um escalar x. Nos expandimos f (x) em uma s´erie de Taylor em torno de algum ponto de opera¸cão (também chamado de ponto de lineariza¸c˜ao), x = ¯x,

definindo ˜x = x− ¯x: f (x) = f (¯x) + ∂f ∂x ¯ x ¯ x + 1 2! ∂2f ∂x2 ¯ x ¯ x2+ 1 3! ∂3f ∂x3 ¯ x ¯ x3 +· · · (21) Agora suponha que x seja um vetor 2× 1. Isto implica que f(x) seja uma fun¸cão não-linear de duas vari´aveis independentes x1 e x2. A expansão da s´erie de Taylor de f (x) torna-se

f (x) = f (¯x) + ∂f ∂x1 ¯ x ˜ x₁ + ∂f ∂x2 ¯ x ˜ x₂+ (22)

(22)

22.