Meggiolaro 1

CONTROLE DISCRETO
DO ACROBOT
©Marco Antonio Meggiolaro
PUC-Rio

Versão 6.0 - 22/05/2014

 Meggiolaro 2

Sistema
• M – massa do carro (0.5 kg)
• m – massa do pêndulo (0.5 kg)
• b – coef. atrito viscoso do carro (0.1 N/(m/s))
• l – comprimento até o centro de massa do pêndulo (0.3m)
• I – momento de inércia do pêndulo (0.006 kgm2)
• f – força aplicada ao carro
• x – posição do carro m, I
q
• teta (q) – ângulo do pêndulo
l
f
M
bx
x
 Meggiolaro 3

Energia Cinética e Potencial

p carro   x 0  , p pendulo   x  l sin q l cos q 
T T

v carro   x 0  , v pendulo   x  lq cos q  lq sin q 

T T

| v pendulo |  (x  lq cos q )  (  lq sin q )  x  2xql  cos q  l q

2 2 2 2 2 2

1 2 1 2 1
T  Mx  m | v pendulo |  Iq 2
2 2 2 m, I
q
 M  m ml cos q  x
1
2

T x q    ml cos q I  ml   q 
2  
l
f
U  mgl cos q M
bx
x
 Meggiolaro 4

Equações do Movimento
 d L L m, I
 dt x  x  f  bx L  TU q
 Lagrangeano
 d L  L  0 l
 dt q q
F M
L
 (M  m)x  mlq cos q bx
x
L
 (I  ml 2 )q  mlx cos q x
q
L L
 0,   mlxq sin q  mgl sin q
x q
 (M  m)x  mlq cos q  mlq 2 sin q  f  bx

 (I  ml )q  mlx cos q  mgl sin q  0
2
 Meggiolaro 5

Linearização das Equações

m, I
 (M  m)x  mlq cos q  mlq sin q  f  bx
2
q

 (I  ml 2
)q  mlx cos q  mgl sin q  0 l
f M
 sin q  q bx
q 0 
 cos q  1 x

 (M  m)x  mlq  f  bx

 (I  ml 2
)q  mlx  mglq  0
 Meggiolaro 6

Função de Transferência
 (M  m)x  bx  mlq  f  U m, I
 q
 (I  ml 2
)q  mlx  mglq  0
l
Laplace
 (M  m)Xs 2  bXs  mls 2  U(s) f M
 bx
 (I  ml )s  mlXs  mgl  0
2 2 2
x
X(s) g I  ml 2
 2
 (s) s ml
X(s)[(M  m)s  bs]   (s)[mls ]  U(s)
2 2

U(s) g I  ml 2
 [(M  m)s  bs][ 2 
2
]  mls 2
 (s) s ml
 Meggiolaro 7

Função de Transferência
q m, I
X(s) g I  ml mgl  (I  ml )s
2 2 2
 2  l
 (s) s ml mls 2 f M
bx
x
 (s)  mls

U(s) (MI  mI  Mml 2 )s 3  (I  ml 2 )bs 2  (M  m)mgl  s  bmgl

X(s) (I  ml 2 )s 2  mgl

U(s) (MI  mI  Mml 2 )s 4  (I  ml 2 )bs 3  (M  m)mgls 2  bmgls
 Meggiolaro 8

Pólos e Zeros de Malha Aberta

acro01.m
M = 0.5; m = 0.2; b = 0.1; l = 0.3; I = 0.006; g = 9.81;

num = [m*l 0];

den = [(M*I+m*I+M*m*l^2) (I+m*l^2)*b -(M+m)*m*g*l -b*m*g*l];
TetaU = tf(num,den);
pole(TetaU) pólos -5.6069, -0.1428, 5.5680
zero(TetaU) zeros 0

num = [I+m*l^2 0 -m*g*l];

den = [(M*I+m*I+M*m*l^2) (I+m*l^2)*b -(M+m)*m*g*l -b*m*g*l 0];
XU = tf(num,den);
pole(XU) pólos -5.6069, -0.1428, 0, 5.5680
zero(XU)
zeros -4.9523, 4.9523
 Meggiolaro 9

Espaço Estado
q  (mglq  mlx) /(I  ml 2 )
m 2l 2 m 2 l 2 gq
[(M  m)  ]  x  bx  f
I  ml 2
I  ml 2

px   (I  ml 2 )bx  m 2l 2gq  (I  ml 2 )f
pq  mlbx  (M  m)mglq  mlf
p  MI  mI  Mml 2

X  FX  GU
 x 0 1 0 0  x  0 
x     
  0  (I  ml 2
)b / p  m 2 2
l g/p 0   x  (I  ml ) / p 
2
 [f ]
q 0 0 0 
1 q  0 
      
 q   0 mlb / p (M  m)mgl / p 0   q   ml / p 
 Meggiolaro 10

Espaço Estado
• assumindo que há somente sensores de posição, que
medem x e q:
Y  HX  JU
x
 
 x  1 0 0 0  x  0
Y      [f ]
q 0 0 1 0 q  0
 
q
• se medisse posição do pêndulo e velocidade do carro:
0 1 0 0 0
H  , J 
0 0 1 0 0
 Meggiolaro 11

Resposta ao Degrau
acro02.m
M = 0.5; m = 0.2; b = 0.1; l = 0.3; I = 0.006; g = 9.81;

p = M*I+m*I+M*m*l^2; %denominator for the F and G matricies

F = [0 1 0 0;
0 -(I+m*l^2)*b/p -(m^2*g*l^2)/p 0;
0 0 0 1;
pólos -5.6069, -0.1428, 0, 5.5680
0 (m*l*b)/p (M+m)*m*g*l/p 0];
G = [0 (I+m*l^2)/p 0 m*l/p]';
H = [1 0 0 0;
0 0 1 0]; %only position sensors
J = [0 0]';
eig(F) %open-loop poles

t = 0:0.05:2;
U = 0.2*ones(size(t));
[Y,X] = lsim(F,G,H,J,U,t);
plot(t,Y)
axis([0 2 -100 100]); legend('x','teta');
xlabel('t'); title('step input for f = 0.2N');
 Meggiolaro 12

Equivalente ZOH
• matrizes equivalentes para período de controle T=10ms
T acro03.m
e FT
,    e ds  G, H, J
Fs T = 1/100; %control period in seconds
[Fi,Gama,H,J] = c2dm(F,G,H,J,T,'zoh')
0
 x(k  1)   1 0.01 0.0001 0   x(k)   0.0001 
 x(k  1)   0 0.9982 0.0267 0.0001   x(k)   0.0182 
     [f (k)]
 q(k  1)   0 0 1.0016 0.01   q(k)   0.0002 
      
 q(k  1)   0 0.0045 0.3122 1.0016   q(k)   0.0454 
 x(k) 
 
 x(k)   1 0 0 0   x(k)   0 
Y(k)         [f (k)]
 q(k)   0 0 1 0   q(k)   0 
 
 q(k) 
 Meggiolaro 13

Controlabilidade e Observabilidade
 H  C      2   3  
 H   
  acro04.m
O
H 2 
co = ctrb (Fi,Gama); Controllability = 4
  ob = obsv (Fi,H);
Observability = 4
 H 3  Controllability = rank (co)
Observability = rank (ob)

• se o carro só tivesse tacômetro, a Observabilidade seria

3, pois não seria possível observar x
• seria possível estimar x a partir de x,
. mas precisaria
conhecer o x inicial e a estimativa poderia ser muito
degradada na presença de ruído na medição de x.
• isso poderia ser aceitável para controlar velocidade do
veículo sem precisão na posição (corrida de acrobots)
 Meggiolaro 14

Controle de Estado com Ackermann

• controle do estado X com matriz de ganho K calculada
por Ackermann para posicionar 4 pólos em 0.95
K   0 0 0 1  C  1   c (  )
• a fórmula de Ackerman acima, e sua equivalente em
Matlab acker(Fi,Gama,P), só valem para sistemas com
apenas 1 entrada (ou seja,  com apenas 1 coluna)
 além disso, elas geram maus resultados para ordem
n > 10 ou para det(C) próximo de zero
• recomenda-se usar a função place(Fi,Gama,P) em
Matlab para posicionar os pólos de malha fechada nos
valores P = [1 2 ... n], ou seja, para que P contenha
os autovalores de {Fi – GamaK}
 a função place não aceita pólos com multiplicidade
maior que o posto de Gama
 Meggiolaro 15

• controle do estado X com matriz de ganho K calculada

por Ackermann para posicionar 4 pólos em 0.95, onde
c(z) é o polinômio cujas raízes são os pólos
 c() é o valor de c(z) quando z = 
acro05.m (pole placement)
co = ctrb (Fi,Gama);
P = [0.95 0.95 0.95 0.95]; %desired closed loop poles
Alfa = poly(P); %coefficients of polyn. with roots P
K = [0 0 0 1]*inv(co)*polyvalm(Alfa,Fi)
K = acker(Fi,Gama,P) %equivalent Matlab function
K = place(Fi,Gama,P) %better Matlab function
eig(Fi-Gama*K) %to check if all poles are 0.95
K = [-14.0254 -11.1099 -44.4151 -8.6785]
• lei de controle a ser implementada (se puder medir X)
U(k ) :  KX  14  x(k )  11  x(k )  44  q(k )  8.7  q(k )
FEEDBACK POSITIVO!
 Meggiolaro 16

• simulação para condição inicial de 1rad/s no pêndulo

acro05.m (cont.) K = [14.0254 -11.1099 -44.4151 -8.6785]
t = 0:0.01:5; %5-second simulation
Uref = 0*ones(size(t)); %zero reference input
X0 = [0 0 0 1]'; %initial condition, 1rad/s speed
[Y,X]=dlsim(Fi-Gama*K, Gama, H, J, Uref, X0); |f|max = 8.7N
fmax = max(abs(Uref'-K*X')); %maximum control effort
stairs(t,Y)
 Meggiolaro 17

• simulação para mesma condição inicial de 1rad/s no

pêndulo mas com os 4 pólos de malha fechada em 0.8
acro06.m
co = ctrb (Fi,Gama); K = [-3590 -664 -1904 -280]
P = [0.8 0.8 0.8 0.8]; %desired closed loop poles
Alfa = poly(P); %coefficients of polynomial with roots P |f|max = 280.9N
K = [0 0 0 1]*inv(co)*polyvalm(Alfa,Fi)
 Meggiolaro 18

• efeito do período de controle T, para mesma condição

inicial e K calibrado para T = 0.01s e pólos em 0.95
K = [14.0254 -11.1099 -44.4151 -8.6785]
acro07.m
for T = [0.001 0.01 0.05 0.08 0.1 0.11 0.115] %control period in seconds
[Fi,Gama,H,J]=c2dm(F,G,H,J,T,'zoh')
K = [-14.0254 -11.1099 -44.4151 -8.6785];
...
[Y,X]=dlsim(Fi-Gama*K, Gama, H, J, U, X0);
stairs(t,Y);
end

• |f|max tende a aumentar com T, para compensar atraso

• o sistema fica instável para T > 0.1098s, quando
eig{K} possui um pólo menor que 1
 pólos 0.95 só seriam obtidos com mesmo K p/ T=0.01s
 o pólo < 1 gera oscilações no carro com período 2T
 Meggiolaro 19

Derivadas Vetoriais e Matriciais

• definições e propriedades matemáticas importantes:
• se f(X) é escalar e X = [x1 x2 ... xn]T é um vetor coluna,
então o Gradiente de f em relação a X é
f  f f f  numerator layout:
 ...  vetor linha
 X   x1  x 2 x n 
 existe uma definição alternativa denominator layout que
retorna o vetor coluna transposto ao definido acima
• no caso particular f(X) = XTAX, com A independente
de X, temos f  (X T AX)
  X T  (A  A T ) (numerator
X X layout)
• se A for simétrica, A = A e assim  (X T
AX)
T
 2X T A
  T X
T T
• para vetor cte. a: (a X)

(X a)
a
X X
 Meggiolaro 20

• se ambos f(X) e X são vetores coluna f = [f1 f2 ... fm]T e

X = [x1 x2 ... xn]T, então a Matriz Jacobiana de f em
relação a X é
 f1 / x1 f1 / x 2 f1 / xn 
 f / x      numerator
f  2 f / x f / x n  layout
 1 2 2 2
X  
 
 fm / x1 fm / x 2 fm / xn 
 nesta definição numerator layout a matriz resultante tem
número de linhas m igual ao (numerador) f
 existe uma definição alternativa denominator layout que
retorna a matriz transposta à definida acima, com número
de linhas n igual ao (denominador) X
• no caso particular f(X) = HX, com H independente de
X, temos F/X = H (numerator layout)
 Meggiolaro 21

• se f(X) é escalar e X = [xij] é uma matriz de n linhas e

m colunas (onde i = 1,...,n e j = 1,...,m), então a
derivada total de f em relação a X é
 f / x11 f / x12 f / x1m 
 f / x      denominator
f  f / x f / x 2m  layout
 21 22
X  
 
 f / xn1 f / xn2 f / xnm 
 nesta definição denominator layout a matriz resultante
tem mesma dimensão n  m que o (denominador) X
 existe uma definição alternativa numerator layout que
retorna a matriz m  n transposta à definida acima
• no caso particular em que o escalar f(X) é o traço tr( )
de alguma matriz (soma dos elementos da diagonal
principal), temos propriedades interessantes
 Meggiolaro 22

• se A for uma matriz independente da matriz X, então as

derivadas totais abaixo satisfazem
denominator layout numerator layout
 tr(AX)  tr(XA)  tr(AX)  tr(XA)
 A T
 A
X X X X
 tr(AX T )  tr(X T A)  tr(AX T )  tr(X T A)
 A   AT
X X X X
 tr(X TAX)  tr(X TAX)
 (A  A T )  X  X T  (A  A T )
X X
 tr(XAX T)  tr(XAX T)
 X  (A  A T )  (A  A T )  X T
X X
 Meggiolaro 23

Controle de Estado com LQR

• controle do estado X com matriz de ganho K calculada
para minimizar a função de custo quadrática linear (e
por isso é chamado de Linear Quadratic Regulator)
1
J   [X(k )T Q1 X(k )  U(k )T Q 2 U(k )]
2 k
• Q1 e Q2 são matrizes de peso simétricas e positivo
semi-definidas, ou seja, todos os seus autovalores são
maiores ou iguais a zero, e portanto J  0
• se o somatório for de 0 a , o problema é para horizonte
(tempo) infinito, chamado de caso regulador
• o problema exige minimizar J dada a “restrição”
X(k  1)    X(k )    U(k )
• seja (k+1) um vetor multiplicador de Lagrange
Meggiolaro 24


1
J(X, U,  )   [X(k)T Q1 X(k)  U(k) T Q 2 U(k) 
2 k 0
 (k  1)T
 (  X(k  1)    X(k)    U(k))]
• derivando em relação ao multiplicador variável (k+1)
e às amostras X(k) e U(k):
J equação
 X(k ) Q 1   (k )   (k  1)   0 adjunta
T T T
X(k )
termo obtido quando k = k1 no somatório
J
 U(k ) Q 2   (k  1)   0 equação de controle
T T
U(k )
J 1
 [  X(k  1)    X(k )    U(k )]  0
T T
 (k  1) 2 equação de estado, logo sempre dá zero
• definindo matrizes S(k) com (k) = S(k)X(k), a
Meggiolaro 25

equação de controle resulta em

U(k ) Q 2   (k  1)   Q 2 U(k )   S(k  1)X(k  1)
T T T

Q 2 U(k )   T S(k  1)  [   X(k )    U(k )]

 
U(k)   [Q 2   TS(k  1) ]1   TS(k  1)  X(k)   K(k)X(k)
• pois Q2T = Q2, e a equação adjunta resulta em
 (k )  Q 1 X(k )   T  (k  1)
S(k )  X(k )  Q 1  X(k )   T  S(k  1)  X(k  1)
S(k )  X(k )  Q 1  X(k )    S(k  1)  [   X(k )    U(k )]
T

• substituindo a lei de controle na equação adjunta,

S(k)X(k)  Q1 X(k)   TS(k  1)X(k)
 TS(k  1)[Q 2   TS(k  1) ]1  TS(k  1)X(k)
 Meggiolaro 26

• isolando X(k) temos

{S(k)  Q1   TS(k  1) 
1 T
 S(k  1)[Q 2   S(k  1) ]  S(k  1) }  X(k)  0
T T

• para ser válido para qualquer X(k), o coeficiente precisa

ser nulo, gerando a equação de recorrência reversa
S(k )  Q1 
 T {S(k  1)  S(k  1)[Q 2   TS(k  1) ]1  TS(k  1)}
• num problema de horizonte finito (otimizando J apenas
para as amostras k de 0 a N), J é minimizado para U(N)
= 0 pois U(N) só influencia X(k>N), que não afeta J,
logo a equação de controle resulta em (N+1) = 0, e
 (N  1)  0   (N)  Q 1 X(N)  S(N)X(N)  S(N)  Q 1
 Meggiolaro 27

• os valores de S(k) são então calculados recursivamente

desde k = N1 até k = 0, usando
S(k )  Q1 
 T {S(k  1)  S(k  1)[Q 2   TS(k  1) ]1  TS(k  1)}
• e a lei de controle ótimo fica com ganho variável
K(k )  [Q 2   T S(k  1) ] 1   T S(k  1)
, k = 0...N1
• no problema de horizonte infinito (regulador), onde
N , o valor de S(k) tende para uma matriz constante
S, obtida da lei de recursão fazendo S = S(k) = S(k+1)
1 T
S  Q 1   {S  S [Q 2   S  ]  S}
T T

• S é a solução da equação discreta de Riccati acima, e o

ganho ótimo passa a ser constante
K  [Q 2   T  S   ] 1   T  S  
 Meggiolaro 28

• em suma, a matriz ótima de ganho K que controla o

estado X por U(k) = KX(k) minimizando
1
J   [X(k )T Q1 X(k )  U(k )T Q 2 U(k )]
2 k
1
• é igual a K  ( S  Q 2 ) ( S )
T T

 onde S é a solução da equação algébrica de Riccati

1 T
S  Q 1   [S  S  (  S   Q 2 )  S]
T T

• para minimizar os erros das variáveis de saída, pode-se

escolher a matriz Q1 de custo de estado igual a HTH, e
a matriz Q2 de desempenho dos atuadores inicialmente
é escolhida como a identidade
 Meggiolaro 29

1 0 0 0
0 0 0 0 
Q1  H T H   , Q 2  [1]
0 0 1 0
 
0 0 0 0
• a matriz Q1 acima vai tentar minimizar o erro em x e q,
enquanto que Q2 vai tentar minimizar a força u = f
requerida pelo controle
 se Q1 fosse escolhida como identidade,
. . tentaria
minimizar também os erros em x e q durante a trajetória,
mas isso só seria relevante em um controle de velocidade
(ao invés de controle de posição), e penalizaria x e q

• a função do Matlab dlqr irá achar a matriz K sujeita às

condições acima, em função de , , Q1 e Q2
 Meggiolaro 30
acro08.m
Q1 = [1 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 0];
Q2 = 1;
K = dlqr(Fi,Gama,Q1,Q2) %digital LQR design
eig(Fi-Gama*K) %para ver os pólos resultantes
• pólos resultantes foram  dois em 0.95 e dois em 0.99
K = [-0.9384 -1.5654 -18.0504 -3.3379]
 Meggiolaro 31

• a resposta foi lenta, e exigiu pouco f, logo vamos

aumentar o peso de q e de x, e sem alterar o peso de f
 500 0 0 0
 0 0 0 0 
Q1  H T H   , Q 2  [1]
 0 0 1000 0 
 
 0 0 0 0
acro09.m
Q1 = [500 0 0 0; 0 0 0 0; 0 0 1000 0; 0 0 0 0];
Q2 = 1;
K = dlqr(Fi,Gama,Q1,Q2) %digital LQR design
K = [-19.9391 -15.0521 -61.8665 -10.7883]
• pólos resultantes foram  0.91 ± 0.07i e 0.98 ± 0.02i
• a resposta é similar à de Ackermann com 4 pólos em
0.95, com |f|max um pouco maior, 10.8N
• lei de controle a ser implementada (se puder medir X)
U(k ) :  KX  20  x(k )  15  x(k )  62  q(k )  11  q(k )
Meggiolaro 32
 Meggiolaro 33

Entrada de Referência
• para posicionar o carro em x ≠ 0, é possível calcular a
referência Uref a ser comandada, onde U : Uref – KX e
X(k  1)  (    K )  X(k )    U ref (k )
• se o valor desejado é Xd, então em regime permanente
X(k  1)  X(k )  X d
X d  (    K )  X d    U ref (k )
  U ref  ( I     K )  X d
U ref  pinv(  )  ( I   )  X d  K  X d
• onde pinv() é a pseudo-inversa de Moore–Penrose,
que no caso vale pinv() = (T)1T (a pseudo-inversa
esquerda de , não use a pseudo-inversa direita pois
pinv() = T(T)1 não é definida se det(T ) = 0)
 Meggiolaro 34

• note que o termo KXd de Uref pode ser incorporado na

lei de controle, enquanto que o primeiro termo precisa
ser nulo para garantir que X realmente tenda a Xd, e não
a algum outro Xd que gere mesmo Uref

U : U ref  KX   K (X  X d )  pinv(  )  ( I   )  X d

• como no nosso problema (I – ) tem primeira coluna

nula, então é possível escolher qualquer Xd = [xd 0 0 0]T
para a posição final desejada, e nesse caso a lei de
controle simplifica para U : – K(X  Xd)
 ou então na forma tradicional U : K(Xd  X)
 Meggiolaro 35

• simulação para condição inicial de 1rad/s no pêndulo

com LQR e valor desejado Xd = [0.5 0 0 0]T
acro10.m
Xd = [0.5 0 0 0]'; %desired position
Uref = pinv(Gama)*(eye(size(Fi)) – Fi)*Xd + K*Xd;
Uref = ones(size(t))*Uref; %reference input |f|max = 2.36N
X0 = [0 0 0 1]'; %initial condition, 1rad/s speed (Xd>0 ajudou)
[Y,X]=dlsim(Fi-Gama*K, Gama, H, J, U, X0);
fmax = max(abs(Uref'-K*X')); %maximum control effort
stairs(t,Y);
 Meggiolaro 36

Projeto do Estimador/Observador
• os controles anteriores assumem que o estado completo
X pode ser medido
• mas ao usar apenas sensores de posição, apenas a saída
Y é medida, onde H foi definida anteriormente
• implementa-se então um estimador preditivo para
prever a estimativa X(k+1) a partir da estimativa
anterior X(k), do valor de U(k), e da medição Y(k)
X(k  1) :   X(k )    U(k )  L p  (Y(k )  H  X(k ))
• pólos de controle 0.91 ± 0.07i e 0.98 ± 0.02i geram
números característicos de amostras 1/(10.91)  11 e
1/(10.98)  50
• deseja-se um estimador 3 a 6 vezes mais rápido que o
controlador, com número característico 11/6  1.8
 Meggiolaro 37

• escolhem-se os 4 pólos do estimador preditivo de malha

fechada próximos de (1.8  1)/1.8 = 0.44
• se houvesse apenas 1 sensor, a matriz H teria apenas 1
coluna e poderia calcular a matriz de ganhos preditivos
Lp por Ackermann (não use se n > 10 ou det(C’)
próximo de zero pois gera erros numéricos)
L p T   0 0 0 1   C ' 1   c (  T )

C'  H T
 H
T T 2T T
 H 3T T 
 H
 
• mas ao usar 2 sensores, é preciso calcular Lp por
 LQR, ajustando os pesos para aproximar os pólos a 0.44
 pela função LpT = place(T, HT, P) do Matlab
• para usar place( ), a multiplicidade das raízes não pode
passar de posto(HT) = 2, logo P = [0.43 0.43 0.44 0.44]
 Meggiolaro 38

• para simular o sistema controlador+estimador (ce), é

preciso escrever as equações da forma
 X(k  1)      K K   X(k )    
 X(k  1)    0        U ref (k )
  L p H   X(k )   0 
  
 X(k  1)   X(k ) 
 X(k  1)    ce   X(k )    ce  U ref (k )
   
• onde o erro da estimativa é definido por
X(k )  X(k )  X(k )
• assumindo que o estado completo inicial [0 0 0 1]T é
conhecido, o erro da estimativa inicialmente é zero
 X(0) 
 X(0)    0 0 0 1 0 0 0 0 
T

 
 Meggiolaro 39
acro11.m
K = dlqr(Fi,Gama,Q1,Q2) %digital LQR design

P = [0.43 0.43 0.44 0.44]; %desired observer closed loop poles

Lp = place(Fi',H',P)' %better Matlab function
eig(Fi-Lp*H) %to check observer poles, same as eig(Fi'-H'*Lp')
t = [0:T:5]'; %5-second simulation
Uref = 0*ones(size(t)); %zero reference input
X0 = [0 0 0 1 0 0 0 0]'; %initial condition of state and estimation error
Fice = [Fi-Gama*K Gama*K;
zeros(size(Fi)) (Fi-Lp*H)];
Gamace = [Gama;
zeros(size(Gama))];
Hce = [H zeros(size(H))];
Jce = J;
[Y,Xce] = dlsim(Fice, Gamace, Hce, Jce, Uref, X0);
fmax = max(abs(Uref'-[K zeros(size(K))]*Xce')); %maximum control effort
stairs(t,Y);
 Meggiolaro 40

• matriz de ganhos do observador obtida:  1.13 0 

 31.7 0.04 
Lp   
 0 1.13 
 
• leis a serem implementadas:  0.44 32.4 
U(k ) :  KX(k )  20  x(k )  15  x(k )  62  q (k )  11  q (k )
X(k  1) :   X(k )    U(k )  L p  (Y(k )  H  X(k ))

• o resultado foi
essencialmente o
mesmo que o do LQR
sem observador, mas
usando 2 sensores a
menos!
 Meggiolaro 41

Simulação sem uso de dlsim()

• a função dlsim() do Matlab só consegue simular
sistemas lineares
• para incluir efeitos não-lineares como os do modelo
sem aproximações e os causados por perturbações ou
efeitos de ruído e quantização nos sensores, é preciso
simular o sistema sem o uso de dlsim()
• para isso, basta escrever sequencialmente as equações
para cada amostra k para calcular os estados e suas
estimativas a partir dos valores anteriores
 X(k ) 
Y(k )  H ce     J ce  U(k )
 X(k ) 
 X(k  1)   X(k ) 
 X(k  1)    ce   X(k )    ce  U ref (k )
   
 Meggiolaro 42

acro12.m
X0 = [0 0 0 1 0 0 0 0]'; %initial condition of state and estimation error
Kce = [K zeros(size(K))]; %controller adapted to the extended state
Xce = X0; Xcevector = []; Yvector = [];
t = [0:T:5]'; %5-second simulation
for t0 = t'
Uref = [0]; %zero reference
Y = Hce*Xce + Jce*(Uref-Kce*Xce); %current sensor reading
Yvector = [Yvector Y]; %stacks Y values
Xcevector = [Xcevector Xce]; %stacks Xce values
Xce = Fice*Xce + Gamace*Uref; %new value of Xce
end
Xce = Xcevector';
Y = Yvector';
fmax = max(abs(Uref'-Kce*Xce')); %maximum control effort
stairs(t,Y);
 Meggiolaro 43

• é conveniente separar as equações da dinâmica do

~
estado X(k) e do erro da estimativa X(k) para inserir
posteriormente efeitos não lineares
• a cada período de controle, as seguintes ações são
executadas (as “simulador” só existem num simulador):
 calcula U(k) a partir da estimativa predita X(k)
U(k ) : U ref  K  X(k ) (controlador)
 simula/mede Y(k), e simula o sistema até X(k+1)
Y(k )  H  X(k )  J  U(k ) (simulador dos sensores)
X(k  1)    X(k )    U(k ) (simulador da dinâmica)
 prediz/estima X(k+1) a partir do U(k) enviado aos
atuadores e do Y(k) medido pelos sensores (estimador)
X(k  1) :   X(k )    U(k )  L p  (Y(k )  H  X(k ))
 Meggiolaro 44

acro13.m
X0 = [0 0 0 1]'; %initial condition of state
X = X0; Xest = X0; %initial actual value and estimate
Urefvector = []; %vector of reference inputs
Xestvector = []; %vector of estimated states
Xvector = []; %vector of actual states
Yvector = []; %vector of measured outputs
t = [0:T:5]'; %5-second simulation
for t0 = t'
Uref = [0]; %zero reference input
U = Uref - K*Xest; %control law
Y = H*X + J*U; %actual sensor reading
Xest = Fi*Xest + Gama*U + Lp*(Y - H*Xest); %observer
Urefvector = [Urefvector Uref]; Xestvector = [Xestvector Xest];
Xvector = [Xvector X]; Yvector = [Yvector Y];
X = Fi*X + Gama*U; %actual system
end
Uref = Urefvector'; Xest = Xestvector'; X = Xvector'; Y = Yvector';
fmax = max(abs(Uref'-K*X')); %maximum control effort
stairs(t,Y);
 Meggiolaro 45

Efeitos de Quantização
• assumindo que o sensor do carro mede valores entre 1
e 1m e que um conversor A/D de b1 bits é usado, sua
resolução (em metros) é q1 = 2 / 2b1
• se o sensor do pêndulo mede entre –p e p com A/D de
b2 bits (ou usa encoder de 2b2 pulsos/volta), a sua
resolução (em radianos) é q2 = 2p / 2b2
• para incorporar este efeito, basta usar na simulação:
Y(k )  H  X(k )  J  U(k )

 Y1 (k)   q1  int(Y1 (k) / q1 ) 

Y(k)     
 2   2
Y (k) q  int(Y2 (k) / q 2 
)
 Meggiolaro 46

acro14.m
q1 = 2/2^b; q2 = 2*pi/2^b;
Urefvector = []; %vector of reference inputs
Xestvector = []; %vector of estimated states
Xvector = []; %vector of actual states
Yvector = []; %vector of measured outputs for perfect sensors
Ysensor = [];
Ysensorvector = []; %vector of actually measured outputs
t = [0:T:5]'; %5-second simulation
for t0 = t'
Uref = [0]; %zero reference input
U = Uref - K*Xest; %control law
Y = H*X + J*U; %reading for ideal sensors
Ysensor(1,1) = q1*floor(Y(1)/q1); %actual sensor readings
Ysensor(2,1) = q2*floor(Y(2)/q2);
Xest = Fi*Xest + Gama*U + Lp*(Ysensor - H*Xest); %observer
Urefvector = [Urefvector Uref]; Xestvector = [Xestvector Xest];
Xvector = [Xvector X]; Yvector = [Yvector Y];
Ysensorvector = [Ysensorvector Ysensor];
X = Fi*X + Gama*U; %actual system
end
 Meggiolaro 47

Matriz de Covariância
• estimadores mais avançados consideram a estatística
dos erros das estimativas, através da variância V (desvio
padrão s ao quadrado) de uma variável escalar x
var(x)  s  E[(x  E(x)) ]  E[x ]  (E[x])
2 2 2 2

 onde E(x) é o valor esperado de x (a média mx de x)

 se por exemplo x for o resultado de um dado não viciado,
mx = E(x) = (1/6)(1+2+3+4+5+6) = 3,5 e
var(x) = (1/6)[(13.5)2 + (23.5)2 +...+ (63.5)2]  2.9
• já a covariância entre 2 escalares x e y mede a relação
entre eles cov(x, y)  E[(x  m x )  (y  m y )]
 onde as médias mx = E[x] e my = E[y]
 note que cov(x, x) = var(x), e se x e y forem variáveis
independentes então cov(x, y) = 0
 Meggiolaro 48

• para um vetor X = [x1 x2 ... xn]T variável com

componentes médias mi  E(xi), define-se a matriz nn
de covariância P = [pij] = cov(X) (onde i, j = 1,...,n) por
p ij  cov(x i , x j )  E[(x i  m i )  (x j  m j )]
 ou, em forma matricial: P  E[(X  m X )  (X  m X ) T ]
onde m X  E[X]  [E[x 1 ] E[x 2 ] E[x n ]]T
 E[(X1  m1 )2 ] E[(X1  m1 )(X 2  m 2 )] E[(X1  m1 )(Xn  m n )] 
 
 E[(X 2  m 2 )(X1  m1 )] E[(X 2  m 2 )2 ] E[(X 2  m 2 )(Xn  m n )]
P 
 
 E[(X  m )(X  m )] E[(X  m )(X  m )] E[(Xn  m n ) 2 ] 
 n n 1 1 n n 2 2 

• P é positivo semi-definida (todos os autovalores i  0)

e, por ser simétrica, seus autovetores ri são ortogonais
• o desvio padrão associado a cada direção ri é si = i
 Meggiolaro 49

• exemplo 1: estado formado pelos valores dado1 e

dado2 de dois dados não-viciados independentes
 dado1   x1   3.5 
X    mX    var(x 1 )  var(x 2 )  2.9
 dado 2   x 2   3.5 
6 6
11
cov(x1 , x 2 )   
pois x1 e x2 são
(i  3.5)(j  3.5)  0 independentes
i 1 j1 6 6
 cov(x1 , x1 ) cov(x1 , x 2 )   2.9 0 
P  cov(X)     
 cov(x ,
2 1x ) cov(x 2 2  
, x ) 0 2.9 
 1   2.9 
    eig(P)   
 2  2.9 
 s1   2.9  1.7  s r2
s     
 2   2.9  1.7  r1

1 0
r1 r2    
 0 1 
 Meggiolaro 50

• exemplo 2: estado formado por dado1 e pela soma dos

dois dados dado1 + dado2
 dado1   x1   3.5   3.5 
X    mX     
 dado1  dado 2   x 2   3.5  3.5   7.0 
var(x 2 )  var(dado1  dado 2 )  var(dado 1 )  var(dado 2 ) 
+2  cov(dado1 , dado 2 )  2.9  2.9  2  0  5.8
6 6
11
cov(x1 , x 2 )  cov(x 2 , x1 )    (i  3.5)(i  j  7.0)  2.9
i 1 j1 6 6
 2.9 2.9 
P 
 2.9 5.8 
r2
 s1    1   1.11  1.06 
r1

s      
 2    2   7.64   2.76 
 0.85 0.53 
r1 r2   
 0.53 0.85 
 Meggiolaro 51

• exemplo 3: estado X(k) = [x1 x2]T forçado pela entrada

U(k) = [dado1 dado2]T, com X(0) = [0 0]T conhecido
 dado1  0 0
X(k  1)  X(k)    P(0)   
 dado 2  0 0 
 a média é mX(k) = [3.5k 3.5k]T e, como X(k) e U(k) são
independentes: cov(X(k  1))  cov(X(k ))  cov(U(k ))
 2.9 0 
P(k  1)  P(k )   
 
0 2.9 amostras
s(3) para k=3
 2.9k 0 
P(k )   
 0 2.9k  s(2)
mX(3)

 s1 (k)   2.9k  s(1)

mX(2)

 s (k)    
 2   2.9k  mX(1)
1 0 mX(0)
evolução
r1 (k) r2 (k)   
do sistema
 0 1 
 Meggiolaro 52

Estimador/Filtro de Kalman (KF)

• se a estatística dos erros dos sensores e de eventuais
perturbações do sistema for conhecida, é possível
calcular matrizes Lp otimizadas por Kalman
• o observador implementado usa a estimativa preditiva e
matriz de ganhos Lp, enquanto que Kalman usa a
estimativa atual uma matriz de ganhos Lc
• Kalman atualiza a estimativa X(k) predita no último
^
intervalo, obtendo um valor corrigido X(k) a partir da
leitura atual dos sensores Y(k), assumindo J = 0
^
• U(k) é calculado a partir da estimativa corrigida X(k)
^
• X(k+1) é previsto a partir de X(k) e U(k)
^
• uma notação usual é Xk|k  X(k) e Xk|k1  X(k)
 Meggiolaro 53

• na ausência de erros nos sensores e de perturbações, a

cada período de controle as seguintes ações são
executadas por um simulador com filtro de Kalman:
 simula/mede Y(k)
Y(k )  H  X(k ) (simulador dos sensores)
 corrige a estimativa X(k), obtendo
X̂(k ) : X(k )  L c  (Y(k )  H  X(k )) (estimador/correção)
^
 calcula U(k) a partir da estimativa corrigida X(k)
ˆ ) (controlador)
U(k ) : U ref  K  X(k
 simula e prevê o valor de X(k+1) por
X(k  1)    X(k )    U(k ) (simulador da dinâmica)
ˆ )    U(k ) (estimador/predição)
X(k  1) :   X(k
• note que Lp = Lc, pois estas equações resultam em
X(k  1) :   X(k )    U(k )    L c  (Y(k )  H  X(k ))
(estimador com correção e predição)
 Meggiolaro 54

X 1) controle U(k) X 2) predição considerando U(k)

(obtido em função de X̂(k ) )

x
X x X x
X(k  1)
x X̂(k) x
x x
x x
t t
X 3) medição Y(k+1) X 4) correção Y(k+1)
(no próximo T) bons sensores
grandes perturbações
x
X̂(k  1)

x X̂(k  1)
X x
x
X x
x
X(k  1)
X(k  1)
x x maus sensores
x x pequenas perturbações
x x
t t
 Meggiolaro 55

• na presença de erros nos sensores e perturbações, o

filtro também precisa atualizar a matriz de covariância
P do estado, que mede a precisão da estimativa
• para isso, o filtro de Kalman assume que
X(k  1)    X(k )    U(k )   w w
Y(k )  H  X(k )  v
 onde w é um vetor n×1 de perturbação aleatória com
matriz de covariância W, e v um vetor de ruído m×1 dos
m sensores com matriz de covariância V
 note que um sensor de resolução q com probabilidade
uniforme entre –q/2 e q/2 tem variância igual a q2/12
p(x)
 q/2 2
1 q
1/q V   x 2  p(x)  dx   x 2  dx 
 q / 2
q 12
x
q/2 q/2
 Meggiolaro 56

 se todas as componentes de perturbação e de ruído forem

variáveis aleatórias independentes, W e V são diagonais
 e, assumindo que w e v possuem média 0, então
W  cov(w)  E[(w  0)  (w  0) T ]  E[w  w T ]
V  cov(v)  E[(v  0)  (v  0) ]  E[v  v ]
T T

 E[w12 ] 0 0   s w12 0 0 
   
 0 E[w 2 2 ] 0   0 s w2 2 0 
W  
   
 0
 0 E[w n 2 ]  0 0 s wn 2 
 E[v12 ] 0 0   s v12 0 0 
   
 0 E[v 2 2 ] 0   0 s v2 2 0 
V  
   
 0
 0 E[v n 2 ]  0 0 s vn 2 

onde swi2 e svi2 significam variância da componente i

 Meggiolaro 57

• a matriz de covariância do estado P = cov(X) é definida

por P = E[(X – E(X))(X – E(X))T], mas depende do
valor real E(X) e portanto não é conhecida exatamente
• definem-se então estimativas preditas e corrigidas da
^ ^
matriz de covariância, P = cov(X) e P = cov(X)
respectivamente, definidas por
P  E[(X  X)  (X  X) ] e Pˆ  E[(X  X)
T ˆ  (X  X)
ˆ T]
 E[(x1  x1 )(x1  x1 )] E[(x1  x1 )(x 2  x2 )] E[(x1  x1 )(x n  xn )] 
 E[(x  x )(x  x )] E[(x  x )(x  x )] E[(x 2  x 2 )(x n  xn )] 
P 2 2 1 1 2 2 2 2
 
 
 E[(xn  xn )(x1  x1 )] E[(xn  xn )(x 2  x2 )] E[(x n  xn )(x n  xn )]

 E[(x1  xˆ 1 )(x1  xˆ 1 )] E[(x1  xˆ 1 )(x 2  xˆ 2 )] E[(x1  xˆ 1 )(x n  xˆ n )] 

 E[(x  xˆ )(x  xˆ )] E[(x  xˆ )(x  xˆ )] E[(x 2  xˆ 2 )(x n  xˆ n )] 
P̂   2 2 1 1 2 2 2 2
 
 
 E[(xn  xˆ n )(x1  xˆ 1 )] E[(xn  xˆ n )(x 2  xˆ 2 )] E[(xn  xˆ n )(x n  xˆ n )]
• cov( ) gera matrizes positivo-definidas simétricas, e
Meggiolaro 58

possui as seguintes propriedades:

cov(X  X ')  cov(X)  cov(X '), se X e X' independentes entre si
cov(AX  a)  A cov(X)A T , para matriz A e vetor a constantes
• conhecendo P(k) := cov(X(k)) previsto na amostra
anterior (k1), e usando cov(Y) = cov(v) = V e
X̂(k ) : (I  L c  H)X(k )  L c Y(k ) , então temos
ˆP(k ) : cov(X(k
ˆ ))  (I  L  H )P(k )(I  L  H ) T  L VL T
c c c c
• assumindo que U(k) é determinístico (não há ruído na
saída D/A pro atuador), então cov(U) = 0 e é possível
predizer P(k+1) := cov(X(k+1)) a partir da estimativa
^
atual corrigida P(k)= ^
cov(X(k))
ˆ (desconhecemos w,
X(k  1) :   X(k )    U(k ) (  w) mas conhecemos W)
w
ˆ )  T    W   T
P(k  1) : cov(X(k  1))    P(k w w
 Meggiolaro 59

• o filtro de Kalman acha Lc para minimizar o erro médio

quadrático (por isso é um Linear Quadratic Estimator)
n
 (X  X )  E[(X  X)  (Xˆ  X)]  tr(P)
ˆ
i
2
i
ˆ T ˆ
i 1
^
^ é o traço (soma dos elementos da diagonal) de P
 tr(P)
^
• expandindo a equação de P(k) temos
P̂(k ) : P(k )  L c HP(k )  P(k )H T L c T  L c HP(k )H TL c T  L c VL c T
• o ganho ideal Lc para minimizar o erro (e o traço) é
tr(P(kˆ ))
 0   2P(k )H T  2L c (HP(k )H T  V)  0
L c
L c : P(k )  H T (HP(k )H T  V )  1
^
• para este ganho ideal, a equação de P(k) simplifica para
P̂(k ) : (I  L c  H)  P(k )
 Meggiolaro 60

• o estimador de Kalman + controlador se resumem em:

 simula/mede Y(k) (ao simular, tem que sortear v e w)
Y(k )  H  X(k )  v (simulador dos sensores)
 calcula o ganho ótimo de Kalman Lc
L c : P(k )  H T (H  P(k )  H T  V )  1 (estimador)
 corrige a última predição X(k) e sua covariância P(k)
X̂(k ) : X(k )  L c  (Y(k )  H  X(k ))
(estimador/correção)
P̂(k ) : (I  L c  H)  P(k )
^
 calcula U(k) a partir da estimativa corrigida X(k)
ˆ ) (controlador)
U(k ) : U ref  K  X(k
 prevê e simula o valor de X(k+1) , e obtém P(k+1), por
ˆ )    U(k )
X(k  1) :   X(k
ˆ
P(k  1) :   P(k )     w  W   w
T T (estimador/predição)

X(k  1)    X(k )    U(k )   w w (simulador da dinâmica)

 Meggiolaro 61

Exemplo para Estimador de Kalman

• M – massa do carro (1 kg)
• f – força aplicada ao carro, U = [f] = 3N
• w – perturbação em f, com desvio padrão sw = .2N
• x – posição real do carro (inicialmente x = 0 e x = 0)
• Y – sensor de posição GPS c/ ruído v de desvio padrão sv =1.5m
x 0 1 x  0 
f  w  Mx           [f  w]
 x   0 0   x  1 / M 
x
Objetivo: estimar com Y  [x  v]  1 0     [v]
T=0.1s a velocidade do carro  x

 x(k  1)   1 0.1  x(k)   0.005   0.005 

 x(k  1)    0 1   x(k)    0.1  [f ]   0.1  [w] f
         M
 x(k ) 
Y(k )  1 0     [v]
 x(k )  x
 Meggiolaro 62

acro15.m
M = 1; F = [0 1; 0 0]; G = [0 1/M]'; H = [1 0]; J = [0];
T = 1/10; %measurement period in seconds
[Fi,Gama,H,J]=c2dm(F,G,H,J,T,'zoh');
Gamaw = Gama; %matrix for disturbance w
vstd = 1.5; V = [vstd^2]; %noise standard deviation and covariance matrix
wstd = 2; W = [wstd^2]; %disturbance standard dev. and covariance matrix
X = [0 0]'; %initial condition of state
Xpred = X; %initial predicted estimate
Ppred = 1e-6*ones(2,2); %initial P (zero if X0 is known)
t = [0:T:5]'; %5-second simulation
for t0 = t'
Y = H*X + (vstd*randn(1)); %GPS sensor reading with noise
Lc = Ppred * H' * inv(H*Ppred*H' + V); %optimal Kalman gain
Xcorr = Xpred + Lc*(Ysensor - H*Xpred); %Kalman observer
Pcorr = (eye(2) - Lc*H) * Ppred; %corrected estimate of P
U = 3; %open loop control (or use Xcorr for closed loop law)
Xpred = Fi*Xcorr + Gama*U; %prediction for next sample
Ppred = Fi*Pcorr*Fi' + Gamaw*W*Gamaw'; %prediction of next P
X = Fi*X + Gama*U + Gamaw*(wstd*randn(1)); %actual X with disturbance
end
 Meggiolaro 63
• Kalman estima muito
bem a posição, mesmo
com ruído e
perturbações aleatórios

• estimativa da
velocidade é muito
ruim sem Kalman,
mesmo com filtro
digital para suavizar
 Meggiolaro 64

Exemplo com Quantização para Kalman

• repetir o problema para U = [f] = 3N sem perturbação (w = 0 e
W = [0]) e com sensor sem ruído (v = 0), porém com resolução
q = 1m, 2m ou 3m
• um sensor de resolução q com probabilidade uniforme entre
–q/2 e q/2 tem variância igual a q2/12, logo V = [q2/12] e
Y(k )  q  int(H  X(k ) / q)
 se o sensor fosse ruidoso de variância sv2, V = [q2/12 + sv2]
acro16.m
wstd = 0; vstd = 0; q = 1; %standard deviations and resolution f
V = [vstd^2+q^2/12]; W = [wstd^2]; %covariance matrices M
for t0 = [0:T:5] %5-second simulation
Y = q*floor(H*X/q); %sensor with quantization
Lc = Ppred*H'*inv(H*Ppred*H'+V); %optimal Kalman gain
Xcorr = Xpred + Lc*(Ysensor - H*Xpred); %Kalman observer x
Pcorr = (eye(2)-Lc*H)*Ppred; %corrected estimate of P
Xpred = Fi*Xcorr + Gama*3; %prediction for next sample with U=3N
Ppred = Fi*Pcorr*Fi' + Gamaw*W*Gamaw'; %prediction of next P
X = Fi*X + Gama*U + Gamaw*wstd*randn(1); %actual system with disturbance
end
 Meggiolaro 65
• Kalman estima a
posição com resolução
melhor que a do sensor

• Kalman estima a
velocidade sem
descontinuidades, que
podem ser grandes sob
pior resolução ou
frequências de
aquisição muito altas
 Meggiolaro 66

Fusão de Sensores com Kalman

• repetir o problema para medir a posição do veículo com 2
sensores redundantes:
 Y1 – sensor de posição GPS com período T = 10ms mas ruído v
com alto desvio padrão sv = 2m, logo V1 = [sv12]
 Y2 – sistema de visão medindo posição com ruído v2 de desvio de
apenas sv2 = 0.5m (V2 = [sv22]), mas alto período T = 10ms
• como as frequências de aquisição são diferentes, é melhor não
juntar as saídas Y1 e Y2 em um único vetor Y, e assim
Y1 (k )  H 1  X(k ) Y2 (k )  H 2  X(k )
• por simplicidade, assuma o carro parado, U = 0, w = 0 e W = [0]
• a cada 10ms, lemos o GPS e fazemos a correção
1
L c : P(k )  H 1 (H 1  P(k )  H 1  V1 )
T T
(ganho ideal)
X̂(k ) : X(k )  L c  (Y1 (k )  H 1  X(k ))
(estimador/correção)
P̂(k ) : (I  L c  H 1 )  P(k )
 Meggiolaro 67
• a cada 100ms, imediatamente após a leitura do GPS lemos a
posição obtida do sistema de visão e fazemos a correção
ˆ )  H T  V )  1 (ganho ideal)
ˆ )  H T (H  P(k
L c : P(k 2 2 2 2
ˆ ) : X(k
X(k ˆ )  L  (Y (k )  H  X(k
ˆ ))
c 2 2 (estimador/correção)
ˆ ) : (I  L  H )  P(k
P(k ˆ )
c 2
^
• note que as equações acima usam os X(k) ^
e P(k) já corrigidos
pelo GPS no mesmo período de controle, ao invés dos X(k) e
P(k) preditos no período anterior
• o Filtro de Kalman permite que um número ilimitado de
sensores redundantes sejam utilizados
 se as frequências forem diferentes, é conveniente separá-los
em vetores e matrizes Yi, Hi e Vi distintos, tomando o
^
cuidado de corrigir a correção X(k) ^
e P(k) (e não a predição
X(k) e P(k)) nos períodos onde mais de um sensor for lido
 se todos possuirem mesma frequência, podem ser agrupados
em vetores e matrizes Y, H e V únicos
 Meggiolaro 68
acro16a.m (simulação de fusão
de sensores para medir uma
posição 50m constante)
• Kalman estima a
posição filtrando os
ruídos dos sensores
GPS e de visão

• Kalman com fusão

dos 2 sensores
estima melhor que
o próprio Kalman
aplicado a apenas
1 dos sensores
 Meggiolaro 69

Controlador LQG
• o controlador LQG (Linear Quadratic Gaussian) nada
mais é do que a combinação do estimador/observador de
Kalman (um LQE baseado em erros gaussianos) com o
controlador/regulador LQR
• consegue-se assim controlar um sistema sem conhecimento
completo dos estados e com perturbações, usando sensores
ruidosos, e com um mínimo “esforço” devido ao LQR
acro17.m (definições)
wstd = 0.01; %stdev of disturbance, in N
vstd1 = 0.001; %stdev of cart sensor errors, in m
vstd2 = 0.001; %stdev of pendulum sensor errors, in rad
q1 = 2/2^12; %12 bits for a position sensor with range 2m
q2 = 2*pi/1024; %pendulum encoder with 1024 pulses/turn
V = [[vstd1^2+q1^2/12 0]
[0 vstd2^2+q2^2/12]]; %noise covariance matrix
W = [wstd^2]; %disturbance covariance matrix
Q1 = [500 0 0 0; 0 0 0 0; 0 0 1000 0; 0 0 0 0]; Q2 = 1;
K = dlqr(Fi,Gama,Q1,Q2) %digital LQR design
 Meggiolaro 70

acro17.m
X0 = [0 0 0 1]'; %initial condition of state
Xpred = X0; %initial prediction estimate
Ppred = 0*ones(n,n); %initial cov. matrix (if unknown, set very small or zero)
X = X0; %state is initially at X0
Xd = [0.5 0 0 0]'; %desired position
Uref = pinv(Gama)*(eye(size(Fi))-Fi)*Xd; %this should be equal to zero
t = [0:T:5]'; %5-second simulation
for t0 = t'
Y = H*X; %reading for ideal position sensor (assume J = 0)
Ysensor(1,1) = q1*floor(Y(1,1)/q1) + vstd1*randn(1); %sensor readings with
Ysensor(2,1) = q2*floor(Y(2,1)/q2) + vstd2*randn(1); %quantization and noise
Lc = Ppred*H'*inv(H*Ppred*H'+V); %optimal Kalman gain
Xcorr = Xpred + Lc*(Ysensor - H*Xpred); %Kalman observer
Pcorr = (eye(n)-Lc*H)*Ppred; %corrected estimate of P
U = Uref - K*(Xcorr-Xd); %closed loop with LQR gain
Xpred = Fi*Xcorr + Gama*U; %prediction for next sample
Ppred = Fi*Pcorr*Fi' + Gamaw*W*Gamaw'; %prediction of next P
X = Fi*X + Gama*U + Gamaw*wstd*randn(1); %actual system with disturb.
end
fmax = max(abs(U')); %maximum control effort
 Meggiolaro 71

• LQG consegue
controlar o
pêndulo mesmo
com ruído e
perturbações
aleatórios e
efeitos de
quantização
• note no entanto
que há um erro
significativo
em regime
permanente
para a posição
x do carro,
causada em
especial pelas
perturbações
 Meggiolaro 72

Controlador LQG + I
• é fácil eliminar o erro de regime permanente do carro,
introduzindo um ganho integral para o erro em x
• a maneira mais fácil de implementá-lo é integrando
numericamente o erro em x dentro do algoritmo de controle,
no entanto a escolha do ganho integral é heurística
 sempre que usar ganho integral em qualquer sistema,
implemente anti-windup e anti-chattering
acro18.m
Xd = [0.5 0 0 0]'; %desired position
errorint = [0 0 0 0]'; %error integral
KI = [K(1)/2 0 0 0]; %integral gain only for x
t = [0:T:5]'; %5-second simulation
for t0 = t'
...
errorint = errorint + (Xcorr-Xd)*T; %numerical integral
U = Uref - K*(Xcorr-Xd) - KI*errorint; %closed loop with LQR gain
...
end
 Meggiolaro 73

• sempre que usar ganho integral, em qualquer sistema,

implemente anti-chattering e anti-windup
if abs(Xcorr(1)-Xd(1)) > 0.5*q1; %anti-chattering for x
errorint(1) = errorint(1) + (Xcorr(1)-Xd(1))*T; %numerical integral
if errorint(1) > 0.4 %anti-windup
errorint(1) = 0.4;
elseif errorint(1) < -0.4
errorint(1) = -0.4;
end
end
if abs(Xcorr(3)-Xd(3)) > 0.5*q2; %anti-chattering for teta
errorint(3) = errorint(3) + (Xcorr(3)-Xd(3))*T; %numerical integral
if errorint(3) > 0.9 %anti-windup
errorint(3) = 0.9;
elseif errorint(3) < -0.9
errorint(3) = -0.9;
end
end
 Meggiolaro 74

• o ganho
integral
consegue
reduzir o erro
de x em regime
permanente, no
entanto
aumenta o
overshoot
• para obter
resposta mais
rápida, seria
necessário usar
matriz de peso
Q1 maior no
cálculo do
ganho pelo
LQR
 Meggiolaro 75

• mas um ganho integral heurístico não garante mais a

otimização obtida pelo LQR
• para calibrar o ganho integral junto do LQR, é preciso
expandir o estado para incorporar a integral da posição do
. . ..
carro c, a qual satisfaz x  c e x  c, e repetir o problema
 a integral do erro seria então  (x  x d )dt  c   x d dt
 c  0 1 0 0 0 c   0 
 c  0 0 1 0 0   c   0 
     
 c   0 0  (I  ml 2 )b / p  m 2l 2g / p 0   c   (I  ml 2 ) / p  [f ]
      
 q  0 0 0 0 1  q   0 
 q   0 (M  m)mgl / p 0   q   ml / p 
 0 mlb / p
c 
c 
 x  0 1 0 0 0   0
Y    c     [f ]
q 0 0 0 1 0   0
q
 q 
 Meggiolaro 76

• a matriz de ganhos K englobará então todos os ganhos PID,

 c  xddt 
otimizados pelo LQR   
 c  xd 
U :  K(X  Xd )    K Ix K Px K Dx K Pq K Dq   c  x 
 d 
 q  qd 
acro19.m (definições)  
 q  qd 
M = 0.5; m = 0.2; b = 0.1; l = 0.3; I = 0.006; g = 9.81;
p = M*I+m*I+M*m*l^2; %denominator for the F and G matricies
F = [0 1 0 0 0; 0 0 1 0 0; 0 0 -(I+m*l^2)*b/p -(m^2*g*l^2)/p 0;
0 0 0 0 1; 0 0 (m*l*b)/p (M+m)*m*g*l/p 0];
G = [0 0 (I+m*l^2)/p 0 -m*l/p]';
H = [0 1 0 0 0; 0 0 0 1 0]; J = [0 0]'; %only position sensors
T = 1/100; %control period in seconds
[Fi,Gama,H,J]=c2dm(F,G,H,J,T,'zoh');
Gamaw = Gama; %matrix for w
...
Q1 = [1000 0 0 0 0; 0 500 0 0 0; 0 0 0 0 0; 0 0 0 1000 0; 0 0 0 0 0]; Q2 = 1;
K = dlqr(Fi,Gama,Q1,Q2) %digital LQR design
X0 = [0 0 0 0 1]'; %initial condition of state
 Meggiolaro 77

acro19.m (cont.)
Xd = [0 0.5 0 0 0]'; %desired position
for t0 = [0:T:5]'; %5-second simulation
Y = H*X; %reading for ideal position sensor (assume J = 0)
Ysensor(1,1) = q1*floor(Y(1,1)/q1) + vstd1*randn(1); %actual sensor readings
Ysensor(2,1) = q2*floor(Y(2,1)/q2) + vstd2*randn(1); %with quantization & noise
Lc = Ppred*H'*inv(H*Ppred*H'+V); %optimal Kalman gain
Xcorr = Xpred + Lc*(Ysensor - H*Xpred); %Kalman observer
Pcorr = (eye(n)-Lc*H)*Ppred; %corrected estimate of P
Xd(1) = Xd(1) + Xd(2)*T; %numerical integral of desired cart position xd
U = Uref - K*(Xcorr-Xd); %closed loop with LQR gain
Xpred = Fi*Xcorr + Gama*U; %prediction for next sample
Ppred = Fi*Pcorr*Fi' + Gamaw*W*Gamaw'; %prediction of next P
X = Fi*X + Gama*U + Gamaw*wstd*randn(1); %actual system with disturb.
end
 xd dt 
 
 xd 
Xd   x   Xd (1)   xddt   xd T  Xd,old (1)  Xd (2)  T
 d 
 qd 
 
q
 d 
 Meggiolaro 78

• o uso do espaço estendido

para calibrar KI pelo
LQG (abaixo) gera
resultados melhores que
os anteriores (à
esquerda), mas exigiram
maior |fmax|

• para penalizar mais o erro

em regime permanente ou
o erro do transiente, basta
alterar os pesos
. relativos
a x  c e x  c da matriz
de custo de estado
Q155 ao calcular K
pelo LQR
 Meggiolaro 79

Simulação Não-Linear
• as simulações anteriores só são válidas para
a linearização feita para pequenos q q m, I
• para avaliar corretamente a eficácia dos
controladores propostos, é preciso simular o l
sistema incluindo todas não-linearidades f M
• a partir das equações do movimento, temos bx
ml(g sin q  x cos q ) x
q
I  ml 2
• e assim
(I  ml 2 )(f  bx  mlq 2 sin q )  m 2l 2g sin q cos q
x
(I  ml )(M  m)  m l cos q
2 2 2 2

(M  m)mlg sin q  bmlx cos q  m 2l 2 q 2 sin q cos q  fml cos q

q
(I  ml 2 )(M  m)  m 2l 2 cos 2 q
 Meggiolaro 80

• o sistema é escrito da forma geral

X  f (X, U)
Y  h (X, U)
 onde f e h são funções não-lineares genéricas de X e U, logo

 x   f1   x 
x  f   [I '(f  bx  mlq sin q)  m l g sin q cos q]/ p
2 2 2 
 
X      2   
q  f 3  q
     
 q   f 4  [(M  m)mlg sin q  bmlx cos q  m 2l 2q 2 sin q cos q  fml cos q]/ p 

x  h1   x 
Y     h (X, U)      
q h 2   q 

I '  (I  ml 2 )
p  I '(M  m)  m 2 l 2 cos 2 q
 Meggiolaro 81

• as funções serão linearizadas localmente em X(k) por uma

aproximação de primeira ordem, tornando F, G, H e J as
matrizes Jacobianas de f e h em relação a X e U

X  X(k )  F(k )  (X  X(k ))  G(k )  (U  U(k ))

Y  Y(k )  H(k )  (X  X(k ))  J(k )  (U  U(k ))

 f 1 / x f 1 / x f 1 / q f 1 / q   f 1 / f 
   
f  f 2 / x f 2 / x f 2 / q f 2 / q  f  f 2 / f 
F  G 
X  f 3 / x f 3 / x f 3 / q f 3 / q  U  f 3 / f 
   
 f 4 / x f 4 / x f 4 / q f 4 / q   f 4 / f 

h  h1 / x h1 / x h1 / q h1 / q  h  h1 / f 

H   J  
X  h2 / x h2 / x h2 / q h2 / q   U   h2 /  f 
• resolvendo para as funções f e h do problema:
Meggiolaro 82

0 1 0 0   0 
0  I 'b / p  f / q 2I 'ml q sin q / p   
F , G  
2 I '/ p 
0 0 0 1   0 
   
 0 mlb cos q / p f 4 / q  m l q sin 2q / p    ml cos q / p 
2 2

f 2
 (I 'mlq 2 cos q  m 2l 2 g cos 2q) / p  m 4l 4g(sin 2 2 q) /(2p 2 ) 
q
 I ' (f  bx  mlq 2 sin q )m 2l 2 (sin 2q ) / p 2
f 4
 [(M  m)m lg cos q  bmlx sin q  m 2l 2q 2 cos 2q  fml sin q]/ p 
q
 [(M  m)mlg sin q  bmlx cos q  m 2l 2 q 2 sin q cos q  fml cos q]  m 2l 2 sin 2 q / p 2

1 0 0 0 0
H  , J 
0 0 1 0 0
 como essas matrizes dependem de X e U, elas precisam ser
recalculadas a cada passo de simulação
 Meggiolaro 83

• simulando o sistema abaixo entre as amostras k e k+1, e

sabendo que U = U(k) = cte. durante esse intervalo, temos
X  X(k )  F(k )  (X  X(k ))  G(k )  (U  U(k )) 0
X  F(k )  X  [ f (X(k ), U(k ))  F(k )X(k )]
X(k  1)    X(k )   0  [ f (X(k ), U(k ))  F(k )X(k )]
T
onde   eFT ,  0   e Fs ds,    0G
0
• assim, para a simulação não-linear, o estado seguinte é
calculado a partir de X(k), U(k), F(k), (k) e 0(k) por
X(k  1)    X(k )   0  [ f (X(k ), U(k ))  F  X(k )]
• o valor lido pelos sensores é calculado diretamente a partir
da equação não-linear
Y(k )  h (X(k ), U(k ))
 Meggiolaro 84

• note que a simulação de um sistema não-linear discreto é

trivial, basta aplicar sequencialmente as equações abaixo
Y(k )  h(X(k ), U(k ))
X(k  1)  f(X(k ), U(k ))
• se as funções não-lineares h e f não são conhecidas, elas
podem ser obtidas a partir das equações anteriores
h(X(k ), U(k ))  h (X(k ), U(k ))
T
f(X(k), U(k))  e FT  X(k)  (  e Fs ds)  [ f (X(k), U(k))  F  X(k)]
0
onde a matriz F é assumida constante entre X(k) e X(k+1),
f
F
X X  X(k )
U  U(k )
 Meggiolaro 85

• note que devido às não-linearidades do sistema, é

aconselhável simulá-lo com um período que seja uma fração
do período T do controlador/estimador, para evitar
instabilidades numéricas
 o período de cada passo da simulação seria Tsim= T/nT, que
seria usado no cálculo de h e f, onde nT é inteiro (nT  1)
 somente a cada nT passos de simulação é feita a atualização
do controle (k:=k+1), com leitura de Y(k) e cálculo de U(k)
acro20.m
(simulação não-linear, sem controle)

• simula-se a seguir o sistema não-linear com o filtro de

Kalman linear já implementado, mas ele fica instável!
acro21.m
(simulação não-linear, com LQR e filtro
de Kalman linear)
 Meggiolaro 86

Filtro de Kalman Estendido (EKF)

• na presença de não-linearidades, as estimativas do filtro de
Kalman linear podem se degenerar e causar instabilidades
• o filtro de Kalman estendido é uma versão não-linear que
corrige estes problemas
• a sua versão tradicional assume que a equação não-linear
discreta do sistema é conhecida, descrita por
X(k  1)  f(X(k ), U(k ), w)
Y(k )  h(X(k ))  v
 onde f e h são funções não-lineares, e w e v possuem média zero
• as matrizes , w e H usadas no filtro de Kalman linear não são
mais constantes, elas são estimadas por
ˆ f ˆ f h
 (k) :  w (k) : H(k) :
X X  X(k
ˆ ) w ˆ )
X  X(k X X  X(k )
 note que dois usam estimativa corrigida e o outro prevista de X
 Meggiolaro 87

• o Kalman estendido + controlador se resumem em:

 simula/mede Y(k) (ao simular, tem que sortear v e w)
Y(k )  h(X(k ))  v (simulador dos sensores)
 calcula a partir de H(k) o ganho ótimo de Kalman Lc
L c : P(k )  H T (H  P(k )  H T  V )  1 (estimador)
 corrige a última predição X(k) e sua covariância P(k)
X̂(k ) : X(k )  L c  [Y(k )  h(X(k ))]
(estimador/correção)
P̂(k ) : (I  L c  H)  P(k )
^
 calcula U(k) a partir da estimativa corrigida X(k)
ˆ )(controlador, pode ser não-linear)
U(k ) : U ref  K  X(k
 prevê e simula o valor de X(k+1) , e obtém P(k+1), por
X(k  1) : f (X(kˆ ), U(k ), w  0) pois E(w)=0
ˆ ˆ ˆ ˆ ˆ T (estimador/predição)
P(k  1) :   P(k )     w  W   w w é sorteado
T

X(k  1)  f (X(k ), U(k ), w ) (simulador da dinâmica)

 Meggiolaro 88
 se w tiver um valor médio wbias não nulo conhecido, é preciso
incluí-lo na equação da predição de X(k+1) (w = wbias  0)
• mas, na maioria dos casos, a equação não-linear contínua
(ao invés da discreta) é conhecida, descrita por
X  f (X, U, w)
Y  h (X)  v
 onde f e h são funções não-lineares
• a equação discreta é então aproximada por
ˆ )  ˆ  [ f (X(k
X(k  1) :   X(k ˆ ), U(k ))  Fˆ  X(k
ˆ )]
0
 onde:
f ˆ f ˆ f h
F̂(k) : , G(k) : , G w (k) : , H(k) :
X X(k
ˆ ) U X(k
ˆ ) w ˆ )
X(k
X X(k )
T
ˆ ˆ ˆ ˆ  ˆ G
ˆ
  e ,  0   e ds, ˆ  ˆ 0G,
ˆ FT ˆ Fs
w 0 w
0
acro22.m (simulação não-linear, com LQR e filtro de Kalman estendido)
 Meggiolaro 89

• o Kalman estendido + controlador nesse caso ficam :

 simula/mede Y(k) (ao simular, tem que sortear v e w)
Y(k )  h (X(k ))  v (simulador dos sensores)
 calcula a partir de H(k) o ganho ótimo de Kalman Lc
L c : P(k )  H T (H  P(k )  H T  V )  1 (estimador)
 corrige a última predição X(k) e sua covariância P(k)
X̂(k ) : X(k )  L c  [Y(k )  h (X(k ))]
(estimador/correção)
P̂(k ) : (I  L c  H)  P(k )
^
 calcula U(k) a partir da estimativa corrigida X(k)
ˆ )(controlador, pode ser não-linear)
U(k ) : U ref  K  X(k
 prevê e simula o valor de X(k+1) , e obtém P(k+1), por
ˆ )  ˆ  [ f (X(k
X(k  1) :   X(k ˆ ), U(k ))  Fˆ  X(k
ˆ )] (estimador/
0
P(k  1) : ˆ  P(k ˆ )  ˆ T  ˆ  W  ˆ T predição)
w w
X(k  1)    X(k )   0  [ f (X(k ), U(k ), w )  F  X(k )] (simulador)
 Meggiolaro 90

• na simulação (realista)
não-linear, o uso do filtro
de Kalman Estendido (à
esquerda) gera bons
resultados, e com valores
reais e estimados
praticamente idênticos ao
longo de toda a simulação

• ao usar o filtro de Kalman

linear (à direita), o sistema
fica instável pois as
estimativas de x e q
divergem dos valores reais
• note que o Kalman linear
consegue estabilizar os
valores estimados, mas
que são irreais
 Meggiolaro 91

Estimador Preditivo
xn

(k )  L p  (Y(k )  H  X(k )) X (k  1)
X(k  1)    X(k )    U
X (k)
Predição

X  [x1 ... x n ]T x1
 Meggiolaro 92

Estimador Preditivo-Corretivo
xn

X̂ (k)
X(k
Correção  1) 
))
 ˆ
)
(k
X(k
HX
(k

)
X


)

Predição U(k
k)
(k
X̂(

)
(Y

X (k) X (k  1)
c
L

X  [x1 ... x n ]T x1
 Meggiolaro 93

Filtro de Kalman Linear (KF)

xn L c : P(k )  H (H  P(k )  H  V ) T T 1 X(k  1)    X(k )    U(k )   w w
Y(k )  H  X(k )  v

X̂ (k) P̂(k) : (I  L c  H)  P(k)

Correção X(k

))
1) 


 ˆ
(k
k)
HX

X(k
X(

)

)
)

U(k
(k
k

ˆ
P(k  1) :   P(k)  T 
X̂(
(Y

)
P(k ) Predição
c
L

w  W  wT
X (k)
sxn sxn
34.1% 34.1%

xn(k)

X (k  1)

34.1% 34.1%
s x1 s x1
x1(k)
X  [x1 ... x n ]T x1
 Meggiolaro 94

Filtro de Kalman Estendido (EKF)

xn L c : P(k )  H (H  P(k )  H  V )
T T 1 X(k  1)  f(X(k ), U(k ), w)
Y(k )  h(X(k ))  v
ˆ (k) : f
 ˆ w (k) :
f
X X  X(k
ˆ ) w ˆ )
X  X(k
h
X̂ (k) P̂(k) : (I  L c  H)  P(k) H(k) :
X X  X(k )

Correção X(k

)]
k) 1) 
) )

f( Xˆ
X(
k
h(
X(

( k ),
)

U(k
(k

P(k  1) :  ˆ
ˆ  P(k)  ˆT
k

))
 [Y

P(k )
X̂(

Predição
ˆ w  W  ˆ w T
c
L

X (k)
sxn sxn
34.1% 34.1%

xn(k)

X (k  1)

34.1% 34.1%
s x1 s x1
x1(k)
X  [x1 ... x n ]T x1
 Meggiolaro 95

Filtro Unscented de Kalman (UKF)

• quando f e h são altamente não-lineares, as equações de
^ da estimativa da matriz P não
predição P e correção P
geram bons resultados, por serem linearizadas
• no filtro unscented, um conjunto de 2n+1 “pontos
sigma” X0(k), X1(k), ..., X2n(k) é escolhido (não é
sorteado) deterministicamente com a ajuda de P(k) a
cada passo ao redor do estado predito X(k)
 esses pontos sigma são escolhidos segundo um algoritmo
específico chamado transformada unscented, a qual os
permite descrever de forma exata a distribuição
Gaussiana armazenada na predição P(k)
 como uma Gaussiana (unidimensional) é totalmente
descrita por apenas 2 parâmetros (média e variância),
2n+1 pontos são mais que suficientes para descrever
unicamente X(k) e P(k) (em n dimensões)
 Meggiolaro 96
 se a distribuição for não-Gaussiana mas seu tipo for
conhecido, é possível usar o UKF, mas os parâmetros da
transformada unscented que escolhe os pontos sigma
precisam ser compatíveis com o tipo da distribuição
 além disso, distribuições mais complexas, cujas equações
envolvam mais de 2 parâmetros, provavelmente
precisarão de mais de 2n+1 pontos no uso do UKF
• o ponto X0(k) é escolhido igual ao próprio X(k), e os
demais pontos sigma se distanciam de X(k) com valores
proporcionais aos desvios-padrão obtidos de P(k)
• na etapa de correção, 2n+1 valores de ruídos v0, v1, ...,
v2n são escolhidos usando a transformada unscented
aplicada à matriz V, onde v0 = 0 é o ruído médio
 os demais vi escolhidos se distanciam de v0 com valores
proporcionais aos desvios-padrão obtidos de V
 Meggiolaro 97

• todos os 2n+1 pontos sigma são então corrigidos de

forma não-linear pelas equações de correção do EKF
usando o mesmo ganho ótimo de Kalman Lc e mesma
leitura real Y(k) dos sensores, mas considerando os
valores escolhidos para os (prováveis) ruídos vi

X̂ i (k ) : X i (k )  L c  [Y(k )  ( h(X i (k ))  v i )] (i  0,1, ..., 2n)

• os pontos sigma corrigidos são então usados para gerar

estimativas de segunda ordem (ou até de terceira, no
^
caso Gaussiano) mais precisas das correções X(k) e
^
P(k), através da transformada unscented inversa
^
 note que, apesar de X0(k)  X(k), a correção X(k)
envolverá uma média ponderada de todos os X ^ (k), e
i
^
portanto, em geral, não será igual a X0(k)
 Meggiolaro 98

Filtro Unscented de Kalman - Correção

xn L c : P(k )  H (H  P(k )  H  V ) T T 1 X(k  1)  f(X(k ), U(k ), w)
Y(k )  h(X(k ))  v
(reconstruído a partir ˆ (k) : f
 ˆ w (k) :
f
P̂(k) dos 2n+1 pontos sigma) X X  X(k
ˆ ) w ˆ )
X  X(k
h
(média ponderada dos H(k) :
X̂ (k) X X  X(k )
2n+1 pontos sigma)
Correção
]
i
v
)) 
h( 
i (k
)
(k i (k
X
 [Y : X
)
)

P(k )
(k
X̂
i
c
L

X (k)
34.1% 34.1%
sxn sxn
xn(k)

pontos
sigma

34.1% 34.1%
s x1 s x1
x1(k)
X  [x1 ... x n ]T x1
 Meggiolaro 99
^ ^ ^
• novos pontos sigma X0(k), X1(k), ..., X2n(k) são então
^
escolhidos ao redor de X(k), usando a transformada
^
unscented baseada na matriz recém-obtida P(k)
• analogamente, 2n+1 valores de perturbações w0, w1, ...,
w2n são escolhidos usando a transformada unscented
aplicada à matriz W, onde w0 = 0 é a perturbação média
• os pontos sigma são então propagados de forma não-
linear pela equação de predição do EKF usando o
mesmo valor da entrada U(k), mas considerando os
valores escolhidos para as (prováveis) perturbações wi
X i (k  1) : f(Xˆ i (k ), U(k ), w i ) (i  0,1, ..., 2n)
• a transformada unscented inversa é então aplicada aos
pontos sigma preditos para gerar estimativas mais
precisas para as predições X(k+1) e P(k+1), e o
processo de correção-predição continua
 Meggiolaro 100

Filtro Unscented de Kalman - Predição

xn L c : P(k )  H (H  P(k )  H  V )
T T 1 X(k  1)  f(X(k ), U(k ), w)
Y(k )  h(X(k ))  v
ˆ (k) : f
novos f
pontos P̂(k)  ˆ w (k) :
X X  X(k
ˆ ) w ˆ )
X  X(k
sigma X̂ (k) h
H(k) :
34.1% 34.1%
sx̂n sx̂n

X X  X(k )
x̂n(k)

X(
i k
1
Predição
) :
f( Xˆ
i (k ) (média ponderada dos
34.1% 34.1% , U( 2n+1 pontos sigma)
s x̂1 s x̂1 k ),
w
i ) X (k  1)
X (k) x̂1(k)

P(k )

P(k  1)
(reconstruído a partir dos
novos 2n+1 pontos sigma)

X  [x1 ... x n ]T x1
 Meggiolaro 101

Filtro Ensemble de Kalman (EnKF)

• em sistemas com um número n muito grande de
variáveis, a matriz P(k) teria ordem nn elevada e sua
atualização seria custosa e com erros numéricos
exemplo: o vetor
de estado no
simulador
meteorológico ao
lado seria
composto da
pressão,
temperatura e das
2 componentes do
vento em cada um
dos 7050 = 3500
pontos da malha
de discretização,
logo n = 43500 =
14000 apenas na
janela da figura!
 Meggiolaro 102

• no EnKF P(k) é estimada por um conjunto (ensemble)

de N vetores de estado iniciais X sorteados, que serão
usados em uma simulação de Monte-Carlo
• cada um dos vetores X1(k), X2(k), ..., XN(k) do
ensemble é simulado/propagado pelas equações do
sistema de forma independente
• a mesma leitura Y(k) dos sensores e o mesmo ganho
ótimo de Kalman Lc são usados na correção das
estimativas dos N vetores
 mas, para capturar os ruídos dos sensores e garantir que
os pontos do ensemble continuem independentes entre si,
ruídos v1, v2, ..., vN são sorteados (e não escolhidos,
como no UKF) usando distribuições Gaussianas com
média zero e desvios padrão extraídos de V
X̂ i (k ) : X i (k )  L c  [Y(k )  ( h(X i (k ))  v i )] (i  1, 2, ..., N )
 Meggiolaro 103

• analogamente, N valores de perturbações w0, w1, ..., wN

são sorteados usando distribuições Gaussianas com
média zero e desvios padrão extraídos de W
• o ensemble é então propagado de forma não-linear pela
equação de predição do EKF usando o mesmo valor da
entrada U(k), mas incluindo perturbações w1, w2, ..., wN
 as perturbações vi são sorteadas (e não escolhidas, como
no UKF) usando distribuições Gaussianas com média
zero e desvios padrão extraídos de W
X i (k  1) : f(Xˆ i (k ), U(k ), w i ) (i  1, ..., N)
• o processo de correção-predição continua, propagando
os mesmos N pontos do ensemble, ao contrário do
UKF, que re-escolhia os pontos simulados (pontos
sigma) em todo e cada passo
 Meggiolaro 104

• o valor do estado pode ser estimado a qualquer instante

a partir da média simples dos N pontos do ensemble
N N
1
ˆX(k)  1  Xˆ (k)
 i X(k  1)    X i (k  1)
N i 1 N i 1
• a matriz de covariância corrigida P(k) ^ não precisa ser
calculada, mas a P(k) predita na amostra k1 precisa
ser extraída do ensemble, pois é usada no cálculo do
ganho ótimo de Kalman Lc
N
1
P(k )    (X i (k )  X(k ))  (X i (k )  X(k )) T
N  1 i 1
 o denominador N1 aparece porque X(k) é a média da
mesma amostra (o ensemble) usada para estimar a
variância, logo é uma medida tendenciosa de E[X(k)]
 se o valor real E[X(k)] fosse conhecido e usado no lugar
de X(k), o denominador seria N na equação acima
 Meggiolaro 105

Filtro Ensemble de Kalman (EnKF)

xn L c : P(k )  H (H  P(k )  H  V )
T T 1 X(k  1)  f(X(k ), U(k ), w)
Y(k )  h(X(k ))  v
ˆ (k) : f
 ˆ w (k) :
f
P̂(k ) : cov(pontos) X X  X(k
ˆ ) w ˆ )
X  X(k
(média simples h
X̂ (k) dos N pontos) H(k) :
X X  X(k )
X(
Correção i k
1
]
i
v
))  ) :
f( Xˆ
h( 

(média simples
i (k
)
(k i (k
X

i (k ) dos N pontos)
 [Y : X

X (k) , U(
)

k ), X (k  1)
)
(k

w
Predição
X̂
i

i )
c
L

P(k )
ensemble
P(k  1) : cov(pontos)
P(k) Gaussiana
X  [x1 ... x n ]T x1
 Meggiolaro 106

• e.g. um sistema de previsão meteorológica pode ter

milhões de graus de liberdade (n > 106), inviabilizando
o uso do UKF, que precisaria propagar mais de 2106
pontos sigma e aplicar a transformada unscented em
matrizes Pnn de trilhões de elementos (n2 > 1012)
• o EnKF seria muito mais eficiente, pois provavelmente
um ensemble de apenas algumas centenas de pontos
(N < 103) seria suficiente para estimar X(k) e P(k)
• o problema da enorme matriz Pnn é resolvido no EnKF
definindo uma matriz auxiliar A que satisfaz P = AAT
A n N   X 1 (k )  X(k ) X 2 (k )  X(k ) ... X N (k )  X(k )  / N  1
L c : PH T (HPH T  V )  1  A  (HA) T (HA  (HA) T  V )  1
 assim, se o número de sensores m << n, o cálculo acima
para Lc é mais eficiente computacionalmente pois Pnn
não precisa ser armazenada, HA tem apenas dimensão
mN, e o cálculo da inversa é sobre uma matriz mm
 Meggiolaro 107

Exemplo de sistema altamente não-linear

P(k) real
X(k)

X(k+1)
EKF

P(k+1) real

EKF não modela bem os efeitos não-lineares em P(k+1)

 Meggiolaro 108

Exemplo de sistema altamente não-linear

P(k) real
sigma
points

EKF
sigma P(k+1) real UKF
points

UKF é melhor que EKF, mas ainda não estima bem P(k+1)
 Meggiolaro 109

Exemplo de sistema altamente não-linear

P(k) real
ensemble

ensemble P(k+1) real EKF

EnKF

EnKF é prediz P(k+1) com muito mais precisão

 Meggiolaro 110

Efeito do número de pontos N do ensemble

EnKF (N=30)
EnKF
(N=300) P(k+1) real

EnKF (N=5)
EKF

quanto maior for o ensemble, mais precisa a previsão

 Meggiolaro 111

Filtro Ensemble de Kalman (EnKF)

 Meggiolaro 112

Filtro Ensemble de Kalman (EnKF)

 Meggiolaro 113

Filtro Ensemble de Kalman (EnKF)

 Meggiolaro 114

Filtro Ensemble de Kalman (EnKF)

 Meggiolaro 115

Filtro Ensemble de Kalman (EnKF)

 Meggiolaro 116

Filtro Ensemble de Kalman (EnKF)

 Meggiolaro 117

KF
 Meggiolaro 118

EnKF
 Meggiolaro 119

EnKF
Meggiolaro 120
Meggiolaro 121
Meggiolaro 122
Meggiolaro 123
 Meggiolaro 124

Particle Filter (PF)

• similar ao EnKF, simulando a evolução de vários
pontos, porém admite qualquer função de distribuição
de probabilidade (fdp) para descrever o estado X, as
perturbações w e o ruído do sensor v
 o PF é recomendável quando há ruído ou perturbações
não-Gaussianas, ou quando a função distribuição de
probabilidade fdp dos estados é muito complexa (função
de vários parâmetros, como a multimodal) ou seu tipo é
completamente desconhecido
• os pontos do ensemble são denominados de partículas,
que são propagados numa simulação sequencial de
Monte-Carlo (SMC) usando as equações do sistema
 como a fdp dos estados não é conhecida, o conjunto
inicial de partículas é sorteado usando uma Gaussiana
 Meggiolaro 125

 mas, ao contrário do EnKF, os sorteios dos ruídos vi e

perturbações wi são feitos a partir de suas fdp, que podem
ser não-Gaussianas (mas o tipo precisa ser conhecido)
• ao invés de estimarem apenas a média X(k) e matriz de
covariância P(k) (como no EnKF), as N partículas
estimam a forma completa da fdp, através de pesos pi
associados a cada uma delas, análogo a um histograma
• o PF precisa de um grande número N de partículas para
funcionar bem na presença de fdp não-Gaussianas
evitando um “empobrecimento” desse conjunto
 o empobrecimento é inevitável
após muitos ciclos, mas quando fdp pesos pi
S pi = 1
p3
ele ocorrer basta descartar as N p2
p4 p
p6 p7
...
partículas e sortear N novas p1
5
pN
(resampling) usando a
estimativa atual da fdp dos x̂ i (k)
estados
 Meggiolaro 126

KF

UKF

PF
 Meggiolaro 127

Filtro de Kalman-Bucy (KBF)

• versão contínua do Kalman linear para controladores
contínuos, usável em discretos se T muito pequeno

• o filtro assume que o sistema segue as equações

X(t)  F  X(t)  G  U(t)  G w  w
Y(t)  H  X(t)  v

 onde F, G e Gw podem ser funções do tempo, e w e v são

perturbações e ruídos aleatórios Gaussianos
 Meggiolaro 128

^ a estimativa atual da matriz de covariância

• sendo P(t)
dos estados, o filtro KBF:
 mede continuamente Y(t) incluindo o ruído Gaussiano v
Y(t)  H  X(t)  v (sensores)
 calcula o ganho ótimo de Kalman-Bucy Lc
L c : P(t)  H T  V  1 (estimador)
^
^ e P(t)
 equações diferenciais das estimativas X(t)
ˆ : F  X(t)
X(t) ˆ  G  U(t)  L  (Y(t)  H  X(t))
ˆ
c (estimador)
ˆ : F  P(t)
P(t) ˆ  P(t)
ˆ  F  G WG  L VL
T T
w w p p
^
 calcula U(t) a partir da estimativa corrigida X(t)
 a dinâmica do sistema vem da equação diferencial
X(t)  F  X(t)  G  U(t)  G w  w (dinâmica)
 Meggiolaro 129

Filtro de Kalman-Bucy Estendido (EKBF)

• versão contínua do Filtro de Kalman Estendido, usável
em sistemas discretos se T muito pequeno
• usa as equações não lineares
X  f (X, U, w)
Y  h (X)  v
• definem-se
f ˆ f ˆ f h
F̂(t) : , G(t) : , G w (t) : , H(t) :
X ˆ
X(t)
U ˆ
X(t)
w ˆ
X(t)
X X(t)

• no KBF a estimativa de P(t) não é afetada pela

estimativa de X(t), porém no EKBF ela é afetada
 assim, os erros nos sensores indiretamente afetam a
estimativa de P(t), pois eles afetam a estimativa de X(t)
 Meggiolaro 130

^ a estimativa atual da matriz de covariância

• sendo P(t)
dos estados, o filtro EKBF:
 mede continuamente Y(t) incluindo o ruído Gaussiano v
Y(t)  h (X(t))  v (sensores)
 calcula o ganho ótimo de Kalman-Bucy Lc
ˆ 1
L c : P(t)  H  V (estimador)
T
^
^ e P(t)
 equações diferenciais das estimativas X(t)
ˆ : f (X(t),
X(t) ˆ ˆ
U(t), t)  L c  [Y(t)  h (X(t))]
(estimador)
ˆ : Fˆ  P(t)
P(t) ˆ ˆ  Gˆ WGˆ
ˆ  P(t)F T T
 L ˆ
HP(t)
w w p
^
 calcula U(t) a partir da estimativa corrigida X(t)
 a dinâmica do sistema vem da equação diferencial
X(t)  f (X(t), U(t), w , t) (dinâmica)
 Meggiolaro 131

Filtro Square Root (SRF)

• usa decomposições de P para garantir que ela continue
positivo semi-definida (todos os autovalores maiores ou
iguais a zero)
 erros numéricos em trajetórias muito longas podem levar
P a ficar com autovalores negativos
• P = SST, onde S é uma matriz triangular (algoritmo de
fatorização de Cholesky)
• P = UDUT, onde U é uma matriz triangular unitária
(triangular com diagonal unitária) e D é uma matriz
diagonal (algoritmo de decomposição U-D, mais
eficiente que o de Cholesky)
• em ambas as opções acima, P sempre resulta em uma
matriz positivo semi-definida
 Meggiolaro 132

Resumo de Filtros de Kalman

• original (KF): discreto, linear, Gaussiano, propaga a
matriz P diretamente e de forma exata (em sistemas
lineares, onde é aplicável)
• estendido (EKF): discreto, não-linear, Gaussiano,
propaga a matriz P diretamente usando uma
aproximação de primeira ordem
• unscented (UKF): discreto, não-linear, Gaussiano (ou
admite alguns não-Gaussianos simples, desde que o tipo
de distribuição seja conhecido), recomendado se f e h
forem altamente não-lineares pois gera aproximação de
terceira ordem para P (ou segunda nos não-Gaussianos)
ao propagar P indiretamente com o auxílio de 2n+1
pontos, chamados pontos “sigma”, que precisam ser re-
escolhidos a cada passo de predição/correção
 Meggiolaro 133

• ensemble (EnKF): discreto, não-linear (pode ser usado

em versão linear), Gaussiano, propaga um “ensemble”
dos mesmos N pontos (não há resampling), estima P a
partir da covariância do ensemble (sem precisar lidar
com transformada unscented inversa), recomendada
para sistemas com muitas variáveis (n muito grande)

• particle (PF): discreto, não-linear, modela qualquer fdp

não-Gaussiana (mesmo se nem o tipo da fdp do estado
for conhecido; porém as fdp não-Gaussianas de w e v
precisam ser conhecidas), propaga N pontos de forma
parecida com o EnKF mas exige resampling após
muitos ciclos, recomendada para problemas não-
Gaussianos
 Meggiolaro 134

• Kalman-Bucy (KBF): contínuo, linear, Gaussiano

• KB estendido (EKBF): contínuo, não-linear, Gaussiano

• filtro square-root (SRF): filtro auxiliar para melhorar a

precisão de qualquer Kalman que use a matriz P

©Marco Antonio Meggiolaro

PUC-Rio

Versão 6.0 - 22/05/2014