ees-20: sistemas de controle iikawakami/ees20/aula28_rev1_handouts.pdf · ees-20: sistemas de...

57
EES-20: Sistemas de Controle II 20 Novembro 2017 1 / 57

Upload: others

Post on 20-Sep-2020

2 views

Category:

Documents


1 download

TRANSCRIPT

Page 1: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

EES-20: Sistemas de Controle II

20 Novembro 2017

1 / 57

Page 2: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Recapitulando: Filtro de Kalman para sistema de 1a ordem

Foi considerado o caso de estado x [k] escalar, com G = 1 e C = 1, porsimplicidade:

Equacao de estado: x [k + 1] = Ax [k] + Bu[k] + w [k]

Equacao de saıda (ou “equacao de medida”): y [k] = x [k] + v [k]

Ruıdos w [k], v [k]: Sequencias de variaveis aleatorias independentes eidenticamente distribuıdas (iid), com distribuicao normal de media zero:

w [k] ∼ N (0,W ) , k = 0, 1, . . .

v [k] ∼ N (0,V ) , k = 0, 1, . . .

2 / 57

Page 3: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Recapitulando: Filtro de Kalman para sistema de 1a ordem

Adota-se a premissa de que x [0] seja uma variavel aleatoria, comdistribuicao gaussiana de media e variancia conhecidas:

x [0] ∼ N(µx[0], σ

2x[0]

)Adicionalmente, considera-se que os ruıdos w [k], v [k] sejam independentesdo estado inicial x [0].

3 / 57

Page 4: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Recapitulando: Filtro de Kalman para sistema de 1a ordem

• Inicializacao:

x [0| − 1] = µx[0], P[0| − 1] = σ2x[0]

• Para cada k ≥ 0 faz-se:

x [k|k] = x [k|k − 1] +P[k|k − 1]

P[k|k − 1] + V(y [k]− x [k|k − 1])

x [k + 1|k] = Ax [k|k] + Bu[k]

P[k|k] = P[k|k − 1]

(1− P[k|k − 1]

P[k|k − 1] + V

)

P[k + 1|k] = A2P[k|k] + W

4 / 57

Page 5: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Recapitulando: Filtro de Kalman para sistema de 1a ordem

Escrevendo

M[k] =P[k|k − 1]

P[k|k − 1] + V

nota-se que as equacoes de atualizacao da estimativa (“measurementupdate” e “time update”) correspondem as do observador de estado, comganho M[k] variante no tempo:

x [k|k] = x [k|k − 1] + M[k](y [k]− x [k|k − 1])

x [k + 1|k] = Ax [k|k] + Bu[k]

5 / 57

Page 6: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Aula de hoje

Deducao das equacoes do Filtro de Kalman considerando que x [k]seja um vetor

Exemplo

6 / 57

Page 7: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Premissas adotadas: Modelo

Equacao de estado: x [k + 1] = Ax [k] + Bu[k] + Gw [k]

Equacao de saıda (ou “equacao de medida”): y [k] = Cx [k] + v [k]

Vetor de estado: x [k] ∈ Rn

Entrada: u[k] ∈ RSaıda medida: y [k] ∈ RPerturbacao (“ruıdo de estado”): w [k] ∈ RRuıdo de medida: v [k] ∈ R

Matrizes do modelo: A ∈ Rn×n, B ∈ Rn×1, C ∈ R1×n, G ∈ Rn×1

Supoe-se (A,C ) observavel e (A,G ) controlavel.

7 / 57

Page 8: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Premissas adotadas: Ruıdos de estado e medida

Ruıdos w [k], v [k]: Sequencias de variaveis aleatorias independentes eidenticamente distribuıdas (iid), com distribuicao normal de media zero:

w [k] ∼ N(0, σ2w

), k = 0, 1, . . .

v [k] ∼ N(0, σ2v

), k = 0, 1, . . .

Notacao adotada para as variancias:

σ2w = W , σ2v = V

8 / 57

Page 9: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Premissas adotadas: Estado inicial

Adota-se a premissa de que x [0] seja um vetor aleatorio, com distribuicaogaussiana multivariada de parametros conhecidos:

x [0] ∼ N(µx[0],Σx[0]

)Adicionalmente, considera-se que os ruıdos w [k], v [k] sejam independentesdo estado inicial x [0].

9 / 57

Page 10: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Distribuicao gaussiana multivariada

Diz-se que o vetor x =[x1 x2 · · · xn

]Ttem distribuicao gaussiana

multivariada:x ∼ N (µ,Σ)

se a densidade de probabilidade conjunta de x1, x2, . . . , xn e da forma

f (x) =1√

det(2πΣ)exp

(− 1

2(x − µ)TΣ−1(x − µ)

)

em que µ =[µ1 µ2 · · · µn

]Te o vetor media:

µ = E{x}

e Σ ∈ Rn×n e uma matriz simetrica e positivo definida de variancias ecovariancias:

Σi ,j = Σj ,i = E{

(xi − µi )(xj − µj)}

10 / 57

Page 11: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Distribuicao gaussiana multivariada

Σi ,j = Σj ,i = E{

(xi − µi )(xj − µj)}

Alternativamente, pode-se escrever

Σ = E{

(x − µ)(x − µ)T}

Por brevidade, vamos nos referir a Σ simplesmente como “matriz decovariancia”.

11 / 57

Page 12: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Distribuicao gaussiana multivariada: Exemplo (n = 2)

µ =

[12

], Σ =

[4 11 9

]

f (x1, x2)

−6

0

8

−6

0

8

0

0.01

0.02

0.03

x1

x2

O que seria o equivalente a um “intervalo de incerteza” no plano x1 × x2 ?12 / 57

Page 13: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Exemplo: Curvas de nıvel

x1

x2

−6 −4 −2 0 2 4 6 8−6

−4

−2

0

2

4

6

8

13 / 57

Page 14: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Curvas de nıvel: Interpretacao

Uma curva de nıvel de f (x) corresponde a um valor constante para aseguinte grandeza:

r(x) = (x − µ)TΣ−1(x − µ)

Propriedade: Se o vetor x =[x1 x2 · · · xn

]T, tem distribuicao gaussiana

multivariada:

x ∼ N (µ,Σ)

entao

r ∼ χ2n

sendo χ2n a chamada distribuicao chi-quadrado com n graus de liberdade.

Matlab: Funcoes chi2cdf e chi2inv14 / 57

Page 15: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Distribuicao chi-quadrado

chi2cdf Chi-square cumulative distribution function.

P = chi2cdf(X,V) returns the chi-square cumulative

distribution function with V degrees of freedom at

the values in X.

chi2inv Inverse of the chi-square cumulative distribution

function (cdf).

X = chi2inv(P,V) returns the inverse of the chi-square

cdf with V degrees of freedom at the values in P.

15 / 57

Page 16: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Funcao de distribuicao chi-quadrado acumulada (n = 2)

>> r = 0:0.01:5; n = 2;

>> chi2 = chi2cdf(r,n);

>> plot(r,chi2)

0 1 2 3 4 50

0.25

0.5

0.75

1

r

F(r

)

16 / 57

Page 17: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Retornando as curvas de nıvel

Exemplo (n = 2):

µ =

[12

], Σ =

[4 11 9

]

Qual a curva de nıvel dentro da qual ha 50% de probabilidade deocorrencia de pares (x1, x2) ?

17 / 57

Page 18: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Retornando as curvas de nıvel

>> chi2inv(0.50,2)

ans =

1.3863

A curva de nıvel e dada por

(x − µ)TΣ−1(x − µ) = 1.3863

18 / 57

Page 19: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Exemplo: Curvas de nıvel

x1

x2

−6 −4 −2 0 2 4 6 8−6

−4

−2

0

2

4

6

8

19 / 57

Page 20: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Retornando ao Filtro de Kalman: Passos a serem seguidos

Passo 1: Obter uma estimativa “a priori” para o estado inicial x [0],denotada por x [0| − 1].

Passo 2: Dado o valor medido de y [0], atualizar a estimativa de modo aobter uma estimativa “a posteriori” x [0|0].

Passo 3: Dada a entrada u[0], propagar a estimativa para o proximoinstante de tempo, obtendo uma nova estimativa “a priori” x [1|0].

Reiterar esse procedimento nos instantes de tempo subsequentes.

20 / 57

Page 21: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 1: Obter a estimativa “a priori” x [0| − 1]

A estimativa x [0| − 1] sera escolhida de modo a minimizar o seguinteerro quadratico medio:

E{

(x [0| − 1])T (x [0| − 1])}

= E{(

x [0]− x [0| − 1])T(

x [0]− x [0| − 1])}

=

∫ (x [0]− x [0| − 1]

)T(x [0]− x [0| − 1]

)f (x [0])dx [0]

Notacao:∫(•) dx ,

∫ ∞x1=−∞

∫ ∞x2=−∞

· · ·∫ ∞xn=−∞

(•) dx1dx2 · · · dxn

21 / 57

Page 22: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 1: Obter a estimativa “a priori” x [0| − 1]

E{

(x [0| − 1])T (x [0| − 1])}

=

∫ (x [0]− x [0| − 1]

)T(x [0]− x [0| − 1]

)f (x [0])dx [0]

Seguindo desenvolvimento similar ao empregado na aula passada, pode-semostrar que o erro quadratico medio e minimizado tomando-se

x [0| − 1] =

∫x [0]f (x [0])dx [0] = E

{x [0]

}= µx[0]

22 / 57

Page 23: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 1: Obter a estimativa “a priori” x [0| − 1]

x [0| − 1] = E{x [0]

}= µx[0]

Como resultado, tem-se

E{

(x [0| − 1])T (x [0| − 1])}

=

∫ (x [0]− µx[0]

)T(x [0]− µx[0]

)f (x [0])dx [0]

= Tr

[∫ (x [0]− µx[0]

)(x [0]− µx[0]

)Tf (x [0])dx [0]

]

= Tr[Σx[0]

]23 / 57

Page 24: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 2: Obter a estimativa “a posteriori” x [0|0]

A estimativa x [0|0] sera escolhida de modo a minimizar o seguinteerro quadratico medio:

E{

(x [0|0])T (x [0|0])∣∣∣ y [0]

}= E

{(x [0]− x [0|0]

)T(x [0]− x [0|0]

) ∣∣∣∣∣ y [0]

}

=

∫ (x [0]− x [0|0]

)T(x [0]− x [0|0]

)f (x [0] | y [0])dx [0]

Seguindo desenvolvimento similar ao empregado na aula passada, pode-semostrar que o erro quadratico medio e minimizado tomando-se

x [0|0] =

∫x [0]f (x [0] | y [0])dx [0] = E

{x [0]

∣∣ y [0]}

E necessario obter a distribuicao condicional f (x [0] | y [0]).24 / 57

Page 25: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 2: Obter a estimativa “a posteriori” x [0|0]

Da equacao de medida no instante k = 0, tem-se que

y [0] = Cx [0] + v [0]

Sabendo que x [0] e v [0] tem distribuicoes gaussianas e sao independentes,

conclui-se que(x [0], y [0]

)tem uma distribuicao conjunta gaussiana.

25 / 57

Page 26: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 2: Obter a estimativa “a posteriori” x [0|0]

A media de y [0] e dada por

E{y [0]

}= E

{Cx [0] + v [0]

}= C E

{x [0]

}+ �����E

{v [0]

}= Cµx[0]

26 / 57

Page 27: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 2: Obter a estimativa “a posteriori” x [0|0]

Sob a premissa de que x [0] e v [0] sao independentes, tem-se ainda:

E{(

y [0]− E{y [0]

})2}= E

{(y [0]− E

{y [0]

})(y [0]− E

{y [0]

})T}= E

{(Cx [0] + v [0]− Cµx[0]

)(Cx [0] + v [0]− Cµx[0]

)T}= E

{(C (x [0]− µx[0]) + v [0]

)(C (x [0]− µx[0]) + v [0]

)T}= C E

{(x [0]− µx[0])(x [0]− µx[0])T

}CT + E

{v2[0]

}= CΣx[0]C

T + V

27 / 57

Page 28: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 2: Obter a estimativa “a posteriori” x [0|0]

Finalmente:

E{(

y [0]− E{y [0]

})(x [0]− E

{x [0]

})T}= E

{(C (x [0]− µx[0]) + v [0]

)(x [0]− µx[0]

)T}= CΣx[0]

28 / 57

Page 29: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 2: Obter a estimativa “a posteriori” x [0|0]

Portanto, pode-se escrever

[x [0]

y [0]

]∼ N

([µx[0]

Cµx[0]

],

[Σx[0] Σx[0]C

T

CΣx[0] CΣx[0]CT + V

])

Deseja-se agora obter f (x [0] | y [0]).

29 / 57

Page 30: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Determinacao de f (x |y)

Sabendo que

f (x ,y) = κxy exp

(− 1

2

[(x− x)T (y− y)

] [ Σ11 Σ12

Σ21 Σ22

]−1 [(x − x)(y − y)

])

f (y) = κy exp

(− 1

2

(y − y)2

Σ22

)

tem-se

f (x |y) =f (x ,y)

f (y)

=κxyκy

exp

{−1

2

([(x−x)T (y−y)

][ Σ11 Σ12

Σ21 Σ22

]−1[(x − x)(y − y)

]−(y − y)2

Σ22

)}30 / 57

Page 31: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Determinacao de f (x |y)

[(x − x)T (y − y)

] [ Σ11 Σ12

Σ21 Σ22

]−1 [(x − x)(y − y)

]− (y − y)2

Σ22

Adotando, por brevidade, a notacao x = (x − x), y = (y − y), a expressaoacima pode ser reescrita como

[xT y

] [ Σ11 Σ12

Σ21 Σ22

]−1 [xy

]− Σ−122 y

2

31 / 57

Page 32: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Determinacao de f (x |y)

Neste ponto, e util recorrer a seguinte identidade:[Σ11 Σ12

Σ21 Σ22

]−1=

[S−1 −S−1Σ12Σ−122

−Σ−122 Σ21S−1 Σ−122 + Σ−122 Σ21S

−1Σ12Σ−122

]

em que

S = Σ11−Σ12Σ−122 Σ21

e o chamado Complemento de Schur de Σ22.

32 / 57

Page 33: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Determinacao de f (x |y)

Empregando essa identidade (e lembrando que Σ21 = ΣT12), pode-se

escrever

[xT y

] [ Σ11 Σ12

Σ21 Σ22

]−1 [xy

]− Σ−122 y

2

=[xT y

] [ S−1 −S−1Σ12Σ−122

−Σ−122 Σ21S−1 Σ−122 + Σ−122 Σ21S

−1Σ12Σ−122

][xy

]− Σ−122 y

2

= xTS−1x − 2xTS−1Σ12Σ−122 y

+ ����Σ−122 y2 + Σ−122 Σ21S

−1Σ12Σ−122 y2 −��

��Σ−122 y2

33 / 57

Page 34: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Determinacao de f (x |y)

= xTS−1x − 2xTS−1Σ12Σ−122 y + Σ−122 Σ21S−1Σ12Σ−122 y

2

= (x − Σ12Σ−122 y)TS−1(x − Σ12Σ−122 y)

Lembrando que x = (x − x) e y = (y − y), chega-se a[x −

(x + Σ12Σ−122 (y − y)

)]TS−1

[x −

(x + Σ12Σ−122 (y − y)

)]Portanto, a distribuicao condicional de x dado o valor de y tem media

x + Σ12Σ−122 (y − y)

e matriz de covariancia

S = Σ11 − Σ12Σ−122 Σ2134 / 57

Page 35: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 2: Obter a estimativa “a posteriori” x [0|0]

Sabendo que[x [0]

y [0]

]∼ N

([µx[0]

Cµx[0]

],

[Σx[0] Σx[0]C

T

CΣx[0] CΣx[0]CT + V

])

Conclui-se que

x [0|0] = E{x [0]

∣∣y [0]}

= µx[0] + Σx[0]CT (CΣx[0]C

T +V )−1(y [0]−Cµx[0])

35 / 57

Page 36: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 2: Obter a estimativa “a posteriori” x [0|0]

A matriz de covariancia da distribuicao condicional de x [0] dado o valor dey [0] e dada por

Σ11 − Σ12Σ−122 Σ21 = Σx[0] − Σx[0]CT (CΣx[0]C

T + V )−1CΣx[0]

=(I − Σx[0]C

T (CΣx[0]CT + V )−1C

)Σx[0]

[Σ11 Σ12

Σ21 Σ22

]=

[Σx[0] Σx[0]C

T

CΣx[0] CΣx[0]CT + V

]

36 / 57

Page 37: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 3: Obter uma nova estimativa “a priori” x [1|0]

Com base no modelo adotado para o sistema, tem-se que

x [1] = Ax [0] + Bu[0] + Gw [0]

A distribuicao condicional de x [1] dado o valor medido de y [0] tambemsera gaussiana, com a seguinte media:

E{x [1]

∣∣∣y [0]}

= E{Ax [0] + Bu[0] + Gw [0]

∣∣∣y [0]}

= AE{x [0]

∣∣∣y [0]}

+ Bu[0] +��������E{Gw [0]

∣∣∣y [0]}

= Ax [0|0] + Bu[0]

e, portanto, a estimativa de x [1] que minimiza o erro quadratico mediosera dada por

x [1|0] = Ax [0|0] + Bu[0]

37 / 57

Page 38: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Passo 3: Obter uma nova estimativa “a priori” x [1|0]

A matriz de covariancia da distribuicao condicional sera dada por

E{(

x [1]− Ax [0|0]− Bu[0])(•)T ∣∣∣y [0]

}= E

{(Ax [0] + ���Bu[0] + Gw [0]− Ax [0|0]−���Bu[0]

)(•)T ∣∣∣y [0]

}= E

{(A(x [0]− x [0|0]) + Gw [0]

)(A(x [0]− x [0|0]) + Gw [0]

)T ∣∣∣y [0]}

= AE{

(x [0]− x [0|0])(x [0]− x [0|0])T∣∣∣y [0]

}AT + GE

{w2[0]

∣∣∣y [0]}GT

= AE{

(x [0]− x [0|0])(x [0]− x [0|0])T∣∣∣y [0]

}AT + GWGT

38 / 57

Page 39: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Reiterar nos proximos instantes de tempo

Para chegar a uma expressao recursiva para o Filtro de Kalman, vamosadotar a seguinte notacao:

P[0| − 1] = E{(

x [0]− x [0| − 1])(x [0]− x [0| − 1]

)T}= E

{(x [0]− µx[0]

)(x [0]− µx[0]

)T}= Σx[0]

P[0|0] = E{(

x [0]− x [0|0])(x [0]− x [0|0]

)T ∣∣y [0]}

=(I − Σx[0]C

T (CΣx[0]CT + V )−1C

)Σx[0]

=(I − P[0| − 1]CT (CP[0| − 1]CT + V )−1C

)P[0| − 1]

39 / 57

Page 40: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Reiterar nos proximos instantes de tempo

P[1|0] = E{(

x [1]− x [1|0])(x [1]− x [1|0]

)T ∣∣y [0]}

= AE{

(x [0]− x [0|0])(x [0]− x [0|0])T∣∣∣y [0]

}AT + GWGT

= AP[0|0]AT + GWGT

Por fim:

x [0|0] = µx[0] + Σx[0]CT (CΣx[0]C

T + V )−1(y [0]− Cµx[0])

= x [0| − 1] + P[0| − 1]CT (CP[0| − 1]CT + V )−1(y [0]− Cx [0| − 1])

40 / 57

Page 41: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Reiterar nos proximos instantes de tempo

Em resumo:

x [0|0] = x [0| − 1] + P[0| − 1]CT (CP[0| − 1]CT + V )−1(y [0]− Cx [0| − 1])

x [1|0] = Ax [0|0] + Bu[0]

P[0|0] =(I − P[0| − 1]CT (CP[0| − 1]CT + V )−1C

)P[0| − 1]

P[1|0] = AP[0|0]AT + GWGT

41 / 57

Page 42: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Reiterar nos proximos instantes de tempo

Nos proximos instantes de tempo, as formulas recursivas serao:

x [k|k] = x [k|k−1] +P[k|k−1]CT (CP[k|k−1]CT+V )−1(y [k]−Cx [k|k−1])

x [k + 1|k] = Ax [k|k] + Bu[k]

P[k|k] =(I − P[k|k − 1]CT (CP[k|k − 1]CT + V )−1C

)P[k|k − 1]

P[k + 1|k] = AP[k|k]AT + GWGT

42 / 57

Page 43: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Reiterar nos proximos instantes de tempo

Escrevendo

M[k] = P[k|k − 1]CT (CP[k|k − 1]CT + V )−1

nota-se que as equacoes de atualizacao da estimativa (“measurementupdate” e “time update”) correspondem as do observador de estado, comganho M[k] variante no tempo:

x [k|k] = x [k|k − 1] + M[k](y [k]− Cx [k|k − 1])

x [k + 1|k] = Ax [k|k] + Bu[k]

Quanto as matrizes de covariancia, tem-se

P[k|k] =(I −M[k]C

)P[k|k − 1]

P[k + 1|k] = AP[k|k]AT + GWGT

43 / 57

Page 44: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Exemplo: Motor eletrico (vide aula de 13 de novembro)

• Estado inicial do motor: x1[0] = 0,1 rad/s e x2[0] = 0,3 A.

• Variancias dos ruıdos: W = (0,5 Nm)2, V = (0,01 rad/s)2

• Estimativa inicial: x1[0| − 1] = 0, x2[0| − 1] = 0.

• Matriz de covariancia “a priori” inicial:

P[0| − 1] =

[(0,2 rad/s)2 0

0 (0,5 A)2

]

44 / 57

Page 45: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Codigo Matlab

P_priori{1} = diag([0.2^2,0.5^2]);

xhat_priori{1} = [0;0];

N = length(y);

for k = 1:N

M{k} = P_priori{k}*C’*inv(C*P_priori{k}*C’ + V);

xhat_posteriori{k} = xhat_priori{k} + ...

M{k}*(y(k) - C*xhat_priori{k});

xhat_priori{k+1} = A*xhat_posteriori{k} + B*u(k);

P_posteriori{k} = (eye(2) - M{k}*C)*P_priori{k};

P_priori{k+1} = A*P_posteriori{k}*A’ + G*W*G’;

end

45 / 57

Page 46: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Resultados

0 0.5 1 1.5 20

0.05

0.1

0.15

0.2

0.25

Tempo (s)

Velo

cid

ade x

1 (

rad/s

)

Estado real

Estimativa a priori

Estimativa a posteriori

46 / 57

Page 47: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Resultados

0 0.5 1 1.5 20

0.05

0.1

0.15

0.2

0.25

0.3

0.35

Tempo (s)

Corr

ente

x2 (

A)

Estado real

Estimativa a priori

Estimativa a posteriori

47 / 57

Page 48: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Comparacao com o resultado obtido na aula de 13 denovembro

>> sigmaw = 0.5; sigmav = 0.01;

>> M = dlqe(A,G,C,sigmaw^2,sigmav^2)

0.7553

-0.2680

48 / 57

Page 49: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Resultado obtido com M constante

0 0.5 1 1.5 20

0.05

0.1

0.15

0.2

0.25

Tempo (s)

Velo

cid

ade x

1 (

rad/s

)

Estado real

Estimativa a priori

Estimativa a posteriori

49 / 57

Page 50: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Resultado obtido com M constante

0 0.5 1 1.5 2−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

Tempo (s)

Corr

ente

x2 (

A)

Estado real

Estimativa a priori

Estimativa a posteriori

50 / 57

Page 51: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Comparacao: Valores de M

0 5 10 15 20 25 300.7

0.75

0.8

0.85

0.9

0.95

1

k

M1(k

)

Filtro de Kalman

Solução estacionária

51 / 57

Page 52: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Comparacao: Valores de M

0 5 10 15 20 25 30−1

0

1

2

3

4

5

6

k

M2(k

)

Filtro de Kalman

Solução estacionária

52 / 57

Page 53: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Explicacao

Na aula de 13 de novembro, a minimizacao do erro quadratico medio parak →∞ resultou em um ganho constante M dado por

M = PCT (CPCT + V )−1

com P = PT > 0 obtida como solucao da seguinte DARE:

P = APAT − APCT (CPCT + V )−1CPAT + GWGT

53 / 57

Page 54: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Explicacao

Ao se considerar a minimizacao do erro quadratico medio a cada instantede tempo, obteve-se um ganho variante no tempo:

M[k] = P[k|k − 1]CT (CP[k|k − 1]CT + V )−1

com

P[k|k] = P[k|k − 1]− P[k|k − 1]CT (CP[k|k − 1]CT + V )−1CP[k|k − 1]

P[k + 1|k] = AP[k|k]AT + GWGT

ou seja, tem-se a seguinte Equacao Recursiva de Riccati:

P[k + 1|k] =

AP[k|k−1]AT−AP[k|k−1]CT(CP[k|k−1]CT+V )−1CP[k|k−1]AT+GWGT

cuja solucao estacionaria corresponde a solucao da DARE.54 / 57

Page 55: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Proxima aula: Prova

Inıcio as 8:00.

Duracao de 100 minutos.

Consulta permitida somente a livros, anotacoes pessoais e materialdistribuıdo durante o curso.

Apoio computacional permitido somente para realizacao de operacoesescalares de soma, subtracao, multiplicacao, divisao, raiz quadrada eavaliacao de exponenciais, logaritmos e funcoes trigonometricas.

Em particular, sera permitido o uso de de apoio computacional pararealizacao de operacoes com numeros complexos, incluindo calculo demodulo e fase.

55 / 57

Page 56: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Exame

Data: 01 Dezembro (6a feira).

Horario de inıcio: 13:30.

Duracao de 3 horas.

Conteudo: Todo o curso.

Orientacoes identicas as das provas mensais.

56 / 57

Page 57: EES-20: Sistemas de Controle IIkawakami/ees20/Aula28_rev1_handouts.pdf · EES-20: Sistemas de Controle II 20 Novembro 2017 1/57. Recapitulando: Filtro de Kalman para sistema de 1a

Muito obrigado pela atencao !

57 / 57