EES-20: Sistemas de Controle II. 20 Novembro 2017

(1)

EES-20: Sistemas de Controle II

(2)

Recapitulando: Filtro de Kalman para sistema de 1a ordem

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

Equa¸c˜ao de estado: x [k + 1] = Ax [k] + Bu[k] + w [k]

Equa¸c˜ao de sa´ıda (ou “equa¸c˜ao de medida”): y [k] = x [k] + v [k]

Ru´ıdos w [k], v [k]: Sequências de variáveis aleatórias independentes e identicamente distribu´ıdas (iid), com distribui¸cão normal de média zero:

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

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

(3)

Recapitulando: Filtro de Kalman para sistema de 1a ordem

Adota-se a premissa de que x [0] seja uma variável aleatória, com distribui¸cão gaussiana de média e variância conhecidas:

x [0] ∼ Nµ_{x [0]}, σ2_{x [0]}

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

(4)

Recapitulando: Filtro de Kalman para sistema de 1a ordem

• Inicializa¸c˜ao:

ˆ

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] = Aˆx [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

(5)

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 equa¸cões de atualiza¸cão da estimativa (“measurement update” e “time update”) correspondem às do observador de estado, com ganho M[k] variante no tempo:

ˆ

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

ˆ

(6)

Aula de hoje

Dedu¸c˜ao das equa¸c˜oes do Filtro de Kalman considerando que x [k] seja um vetor

Exemplo

(7)

Premissas adotadas: Modelo

Equa¸c˜ao de estado: x [k + 1] = Ax [k] + Bu[k] + Gw [k]

Equa¸c˜ao de sa´ıda (ou “equa¸c˜ao de medida”): y [k] = Cx [k] + v [k]

Vetor de estado: x [k] ∈ Rn Entrada: u[k] ∈ R

Sa´ıda medida: y [k] ∈ R

Perturba¸c˜ao (“ru´ıdo de estado”): w [k] ∈ R Ru´ıdo de medida: v [k] ∈ R

Matrizes do modelo: A ∈ Rn×n, B ∈ Rn×1, C ∈ R1×n, G ∈ Rn×1 Supõe-se (A, C ) observável e (A, G ) controlável.

(8)

Premissas adotadas: Ru´ıdos de estado e medida

Ru´ıdos w [k], v [k]: Sequências de variáveis aleatórias independentes e identicamente distribu´ıdas (iid), com distribui¸cão normal de média zero:

w [k] ∼ N 0, σ_w2 , k = 0, 1, . . . v [k] ∼ N 0, σ2_v , k = 0, 1, . . . Nota¸c˜ao adotada para as variˆancias:

σ_w2 = W , σ_v2 = V

(9)

Premissas adotadas: Estado inicial

Adota-se a premissa de que x [0] seja um vetor aleatório, com distribui¸cão gaussiana multivariada de parâmetros conhecidos:

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

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

(10)

Distribui¸c˜

ao gaussiana multivariada

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

T

tem distribui¸c˜ao gaussiana multivariada:

x ∼ N (µ, Σ)

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

f (x ) = 1 pdet(2πΣ)exp − 1 2(x − µ) T_Σ−1_{(x − µ)} ! em que µ = µ1µ2 · · · µn T ´ e o vetor m´edia: µ = Ex

e Σ ∈ Rn×n é uma matriz simétrica e positivo definida de variâncias e covariâncias:

Σi ,j = Σj ,i = E(xi − µi)(xj − µj)

(11)

Distribui¸c˜

ao 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 de covariˆancia”.

(12)

Distribui¸c˜

ao gaussiana multivariada: Exemplo (n = 2)

µ = 1 2 , Σ = 4 1 1 9 f (x1, x2) −6 0 8 −6 0 8 0 0.01 0.02 0.03 x 1 x 2

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

(13)

Exemplo: Curvas de n´ıvel

x 1 x 2 −6 −4 −2 0 2 4 6 8 −6 −4 −2 0 2 4 6 8

(14)

Curvas de n´ıvel: Interpreta¸c˜

ao

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

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

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

T

, tem distribui¸c˜ao gaussiana multivariada:

x ∼ N (µ, Σ) ent˜ao

r ∼ χ2_n

sendo χ2_n a chamada distribui¸c˜ao chi-quadrado com n graus de liberdade.

Matlab: Fun¸c˜oes chi2cdf e chi2inv

(15)

Distribui¸c˜

ao 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.

(16)

Fun¸c˜

ao de distribui¸c˜

ao chi-quadrado acumulada (n = 2)

>> r = 0:0.01:5; n = 2; >> chi2 = chi2cdf(r,n); >> plot(r,chi2) 0 1 2 3 4 5 0 0.25 0.5 0.75 1 r F(r) 16 / 57

(17)

Retornando `

as curvas de n´ıvel

Exemplo (n = 2): µ = 1 2 , Σ = 4 1 1 9

Qual a curva de n´ıvel dentro da qual h´a 50% de probabilidade de ocorrˆencia de pares (x1, x2) ?

(18)

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

(19)

Exemplo: Curvas de n´ıvel

x 1 x 2 −6 −4 −2 0 2 4 6 8 −6 −4 −2 0 2 4 6 8

(20)

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 a obter uma estimativa “a posteriori” ˆx [0|0].

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

Reiterar esse procedimento nos instantes de tempo subsequentes.

(21)

Passo 1: Obter a estimativa “a priori” ˆ

x [0| − 1]

A estimativa ˆx [0| − 1] será escolhida de modo a minimizar o seguinte erro quadrático médio:

(22)

Passo 1: Obter a estimativa “a priori” ˆ

x [0| − 1]

En(˜x [0| − 1])T(˜x [0| − 1])o

= Z

x [0] − ˆx [0| − 1]Tx [0] − ˆx [0| − 1]f (x [0])dx [0]

Seguindo desenvolvimento similar ao empregado na aula passada, pode-se mostrar que o erro quadrático médio é minimizado tomando-se

ˆ

x [0| − 1] = Z

x [0]f (x [0])dx [0] = Ex[0] = µx [0]

(23)

Passo 1: Obter a estimativa “a priori” ˆ

x [0| − 1]

ˆ

x [0| − 1] = Ex[0] = µx [0]

Como resultado, tem-se

E n (˜x [0| − 1])T(˜x [0| − 1]) o = Z x [0] − µx [0] T x [0] − µx [0] f (x [0])dx [0] = Tr " Z x [0] − µ_{x [0]}x [0] − µ_{x [0]}Tf (x [0])dx [0] # = TrhΣ_{x [0]}i

(24)

Passo 2: Obter a estimativa “a posteriori” ˆ

x [0|0]

A estimativa ˆx [0|0] será escolhida de modo a minimizar o seguinte erro quadrático médio:

En(˜x [0|0])T(˜x [0|0]) y [0] o = E ( x [0] − ˆx [0|0]Tx [0] − ˆx [0|0] y [0] ) = Z x [0] − ˆx [0|0]Tx [0] − ˆx [0|0]f (x [0] | y [0])dx [0]

Seguindo desenvolvimento similar ao empregado na aula passada, pode-se mostrar que o erro quadrático médio é minimizado tomando-se

ˆ x [0|0] = Z x [0]f (x [0] | y [0])dx [0] = Ex[0]y [0] ´

E necess´ario obter a distribui¸c˜ao condicional f (x [0] | y [0]).

(25)

Passo 2: Obter a estimativa “a posteriori” ˆ

x [0|0]

Da equa¸c˜ao de medida no instante k = 0, tem-se que

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

Sabendo que x [0] e v [0] têm distribui¸cões gaussianas e são independentes, conclui-se que x [0], y [0]têm uma distribui¸cão conjunta gaussiana.

(26)

Passo 2: Obter a estimativa “a posteriori” ˆ

x [0|0]

A m´edia de y [0] ´e dada por

Ey [0] = E Cx[0] + v [0] = C E x[0] +

Ev [0] = C µx [0]

(27)

Passo 2: Obter a estimativa “a posteriori” ˆ

x [0|0]

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

En y [0] − Ey [0] 2o= En y [0] − Ey [0] y [0] − E y [0] To = En Cx [0] + v [0] − C µ_{x [0]} Cx [0] + v [0] − C µ_{x [0]}To = E n C (x [0] − µx [0]) + v [0] C (x [0] − µx [0]) + v [0] To = C En(x [0] − µ_{x [0]})(x [0] − µ_{x [0]})ToCT + Env2[0]o = C Σ_{x [0]}CT + V

(28)

Passo 2: Obter a estimativa “a posteriori” ˆ

x [0|0]

Finalmente: E n y [0] − Ey [0] x[0] − E x[0] T o = EnC (x [0] − µx [0]) + v [0] x [0] − µx [0] To = C Σx [0] 28 / 57

(29)

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]CT C Σx [0] C Σx [0]CT+ V #!

(30)

Determina¸c˜

ao de f (x |y )

Sabendo que f (x ,y ) = κxyexp − 1 2 h (x − ¯x )T (y − ¯y ) i " Σ11 Σ12 Σ21 Σ22 #−1 (x − ¯x ) (y − ¯y ) ! f (y ) = κyexp − 1 2 (y − ¯y )2 Σ22 ! tem-se f (x |y ) = f (x ,y ) f (y ) = κxy κy exp ( −1 2 h (x −¯x )T (y −¯y ) i " Σ11 Σ12 Σ21 Σ22 #−1 (x − ¯x ) (y − ¯y ) −(y − ¯y ) 2 Σ22 !) 30 / 57

(31)

Determina¸c˜

ao de f (x |y )

h (x − ¯x )T (y − ¯y ) i " Σ11 Σ12 Σ21 Σ22 #−1 (x − ¯x ) (y − ¯y ) −(y − ¯y ) 2 Σ22

Adotando, por brevidade, a nota¸c˜ao ˜x = (x − ¯x ), ˜y = (y − ¯y ), a express˜ao acima pode ser reescrita como

h ˜ xT ˜y i " Σ11 Σ12 Σ21 Σ22 #−1 ˜ x ˜ y − Σ−1₂₂y˜2

(32)

Determina¸c˜

ao de f (x |y )

Neste ponto, é útil recorrer à seguinte identidade:

" Σ11 Σ12 Σ21 Σ22 #−1 = " S−1 −S−1Σ12Σ−1₂₂ −Σ−1₂₂Σ21S−1 Σ−122 + Σ −1 22Σ21S−1Σ12Σ−122 # em que S = Σ11−Σ12Σ−1₂₂Σ21 ´

e o chamado Complemento de Schur de Σ22.

(33)

Determina¸c˜

ao de f (x |y )

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

escrever h ˜ xT ˜y i " Σ11 Σ12 Σ21 Σ22 #−1 ˜ x ˜ y − Σ−1₂₂y˜2 =h˜xT ˜yi " S−1 −S−1_Σ 12Σ−122 −Σ−1₂₂Σ21S−1 Σ−122 + Σ −1 22Σ21S−1Σ12Σ−122 # ˜ x ˜ y − Σ−1₂₂y˜2 = ˜xTS−1˜x − 2˜xTS−1Σ12Σ−1₂₂˜y + Σ−1₂₂˜y2 + Σ−1₂₂Σ21S−1Σ12Σ−1₂₂y˜2− Σ−1₂₂˜y2

(34)

Determina¸c˜

ao de f (x |y )

= ˜xTS−1˜x − 2˜xTS−1Σ12Σ−122˜y + Σ −1 22Σ21S−1Σ12Σ−122˜y2 = (˜x − Σ12Σ−122˜y )TS −1_(˜_{x − Σ} 12Σ−122y )˜

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

h

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

iT

S−1hx − ¯x + Σ12Σ−122(y − ¯y )

i

Portanto, a distribui¸c˜ao condicional de x dado o valor de y tem m´edia

¯

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

e matriz de covariˆancia

S = Σ11− Σ12Σ−1₂₂Σ21

(35)

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]CT C Σ_{x [0]} C Σ_{x [0]}CT+ V #! Conclui-se que ˆ x [0|0] = Ex[0]y [0] = µx [0]+ Σx [0]CT(C Σx [0]CT+ V )−1(y [0] − C µx [0])

(36)

Passo 2: Obter a estimativa “a posteriori” ˆ

x [0|0]

A matriz de covariância da distribui¸cão condicional de x [0] dado o valor de y [0] é dada por Σ11− Σ12Σ−1₂₂Σ21= Σx [0]− Σx [0]CT(C Σx [0]CT+ V )−1C Σx [0] =I − Σx [0]CT(C Σx [0]CT+ V )−1C Σx [0] " Σ11 Σ12 Σ21 Σ22 # = " Σx [0] Σx [0]CT C Σ_{x [0]} C Σ_{x [0]}CT+ V # 36 / 57

(37)

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 distribui¸cão condicional de x [1] dado o valor medido de y [0] também será gaussiana, com a seguinte média:

E n x [1] y [0] o = E n Ax [0] + Bu[0] + Gw [0] y [0] o = A E n x [0] y [0] o + Bu[0] + E n Gw [0] y [0] o = Aˆx [0|0] + Bu[0]

e, portanto, a estimativa de x [1] que minimiza o erro quadrático médio será dada por

ˆ

(38)

Passo 3: Obter uma nova estimativa “a priori” ˆ

x [1|0]

A matriz de covariância da distribui¸cão condicional será dada por

(39)

Reiterar nos pr´

oximos instantes de tempo

Para chegar a uma express˜ao recursiva para o Filtro de Kalman, vamos adotar a seguinte nota¸c˜ao:

(40)

Reiterar nos pr´

oximos instantes de tempo

P[1|0] = En x [1] − ˆx [1|0] x [1] − ˆx [1|0]T y [0]o = A E n (x [0] − ˆx [0|0])(x [0] − ˆx [0|0])T y [0] o AT + GWGT = AP[0|0]AT + GWGT Por fim: ˆ x [0|0] = µx [0]+ Σx [0]CT(C Σx [0]CT+ V )−1(y [0] − C µx [0]) = ˆx [0| − 1] + P[0| − 1]CT(CP[0| − 1]CT+ V )−1(y [0] − C ˆx [0| − 1]) 40 / 57

(41)

Reiterar nos pr´

oximos instantes de tempo

Em resumo: ˆ x [0|0] = ˆx [0| − 1] + P[0| − 1]CT(CP[0| − 1]CT+ V )−1(y [0] − C ˆx [0| − 1]) ˆ x [1|0] = Aˆx [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

(42)

Reiterar nos pr´

oximos instantes de tempo

Nos próximos instantes de tempo, as fórmulas recursivas serão:

(43)

Reiterar nos pr´

oximos instantes de tempo

Escrevendo

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

nota-se que as equa¸cões de atualiza¸cão da estimativa (“measurement update” e “time update”) correspondem às do observador de estado, com ganho M[k] variante no tempo:

ˆ

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

ˆ

x [k + 1|k] = Aˆx [k|k] + Bu[k]

Quanto `as matrizes de covariˆancia, tem-se

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

(44)

Exemplo: Motor el´

etrico (vide aula de 13 de novembro)

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

• Variˆancias 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 covariˆancia “a priori” inicial:

P[0| − 1] = (0,2 rad/s)2 0 0 (0,5 A)2 44 / 57

(45)

C´

odigo 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

(46)

Resultados

0 0.5 1 1.5 2 0 0.05 0.1 0.15 0.2 0.25 Tempo (s) Velocidade x 1 (rad/s) Estado real Estimativa a priori Estimativa a posteriori 46 / 57

(47)

Resultados

0 0.5 1 1.5 2 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 Tempo (s) Corrente x 2 (A) Estado real Estimativa a priori Estimativa a posteriori

(48)

Compara¸c˜

ao com o resultado obtido na aula de 13 de

novembro

>> sigmaw = 0.5; sigmav = 0.01; >> M = dlqe(A,G,C,sigmaw^2,sigmav^2) 0.7553 -0.2680 48 / 57

(49)

Resultado obtido com M constante

0 0.5 1 1.5 2 0 0.05 0.1 0.15 0.2 0.25 Tempo (s) Velocidade x 1 (rad/s) Estado real Estimativa a priori Estimativa a posteriori

(50)

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) Corrente x 2 (A) Estado real Estimativa a priori Estimativa a posteriori 50 / 57

(51)

Compara¸c˜

ao: Valores de M

0 5 10 15 20 25 30 0.7 0.75 0.8 0.85 0.9 0.95 1 k M 1 (k) Filtro de Kalman Solução estacionária

(52)

Compara¸c˜

ao: Valores de M

0 5 10 15 20 25 30 −1 0 1 2 3 4 5 6 k M 2 (k) Filtro de Kalman Solução estacionária 52 / 57

(53)

Explica¸c˜

ao

Na aula de 13 de novembro, a minimiza¸cão do erro quadrático médio para k → ∞ resultou em um ganho constante M dado por

M = PCT(CPCT+ V )−1

com P = PT > 0 obtida como solu¸c˜ao da seguinte DARE:

(54)

Explica¸c˜

ao

Ao se considerar a minimiza¸cão do erro quadrático médio a cada instante de 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 Equa¸c˜ao 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 solu¸cão estacionária corresponde à solu¸cão da DARE.

(55)

Pr´

oxima aula: Prova

In´ıcio `as 8:00.

Dura¸c˜ao de 100 minutos.

Consulta permitida somente a livros, anota¸c˜oes pessoais e material distribu´ıdo durante o curso.

Apoio computacional permitido somente para realiza¸cão de opera¸cões escalares de soma, subtra¸cão, multiplica¸cão, divisão, raiz quadrada e avalia¸cão de exponenciais, logaritmos e fun¸cões trigonométricas. Em particular, será permitido o uso de de apoio computacional para realiza¸cão de opera¸cões com números complexos, incluindo cálculo de módulo e fase.

(56)

Exame

Data: 01 Dezembro (6a feira). Horário de in´ıcio: 13:30. Dura¸cão de 3 horas. Conteúdo: Todo o curso.

Orienta¸cões idênticas às das provas mensais.

(57)