-
Controle Ótimo - Aula 5Filtro de Kalman
Adriano A. G. Siqueira e Marco H. Terra
Departamento de Engenharia Elétrica
Universidade de São Paulo - São Carlos
Controle Ótimo - Aula 5Filtro de Kalman – p.1/52
-
Informação imperfeita
Caso de informação perfeita, a política ótima consistia em realimentaro estado através de um atuador linear com relação ao estado
uk = Lkxk
No caso de informação imperfeita, quando o estado não é acessível,utiliza-se a realimentação de uma estimativa do estado
uk = LkE{xk|Ik}, Ik = (zk, zk−1, ..., z0, uk−1, ..., u0)
Implementação de um estimador
x̂k = E{xk|Ik}
Controle Ótimo - Aula 5Filtro de Kalman – p.2/52
-
Estabelecimento do problema
Considere o seguinte sistema linear
xk+1 = Akxk + wk, k = 0, .., N − 1
sendo xk ∈ Rn o estado, wk ∈ Rn o distúrbio aleatório, e as matrizesAk ∈ R
n×n são conhecidas. Note que o vetor de controle não aparecepois vimos que o erro de estimativa não depende do sinal de controle.
Vamos assumir que o estado xk não é disponível, mas que temos a cadainstante a seguinte medida do estado
zk = Ckxk + vk, k = 0, .., N − 1
sendo zk ∈ Rs o vetor das variáveis observadas, vk ∈ Rs o ruídoaleatório de medida.
Controle Ótimo - Aula 5Filtro de Kalman – p.3/52
-
Estabelecimento do problema
Para os vetores aleatórios independentes, x0, wk, vk, assumimos que
H1) a condição inicial é gaussiana com covariância S :
E{(x0 − E{x0}) (x0 − E{x0})T} = S
H2) o distúrbio wk é gaussiano com média zero e branco:
E{wk} = 0, E{wkwTk } = Mk ≥ 0
H3) o distúrbio vk é gaussiano com média zero e branco:
E{vk} = 0, E{vkvTk } = Nk > 0
Controle Ótimo - Aula 5Filtro de Kalman – p.4/52
-
Estabelecimento do problema
O problema é implementar um estimador que produz o valor esperadode xk condicionado às medidas zk, zk−1, ..., z0
x̂k = E{xk|zk, zk−1, ..., z0} = E{xk|Zk}
Note que como não temos entrada de controle, a informação que temosé dada por Zk :
Ik = (zk, zk−1, ..., z0, uk−1, ..., u0) = (zk, zk−1, ..., z0) = Zk
Controle Ótimo - Aula 5Filtro de Kalman – p.5/52
-
Estimativa Mínima Quadrática
Problema geral de estimativa estocástica
Considere duas variáveis aleatórias (escalares ou vetores coluna) x e ycom função densidade de probabilidade f(x, y) conhecida. Oproblema geral de estimação estocástica consiste na seguinte pergunta:
Dado que a variável y assumiu um determinado valor, o que pode serdito a respeito do valor assumido pela variável x ?
Estamos interessados em obter uma função x (.), chamada estimador,tal que
x̂ = x (y)
é a estimativa do valor assumido por x dado y
Controle Ótimo - Aula 5Filtro de Kalman – p.6/52
-
Estimativa Mínima Quadrática
O estimador deve ser ótimo segundo o critério do erro quadráticomínimo
minx(y)Ex,y{‖x − x (y)‖2}
Vamos mostrar que o estimador E{x|y} é um estimador ótimo
Proposição E.1: O estimador ótimo quadrático para a variável x dadoo valor da variável y é dado por
x∗(y) = Ex{x|y}
Controle Ótimo - Aula 5Filtro de Kalman – p.7/52
-
Estimativa Mínima Quadrática
Prova: Vamos usar a seguinte propriedade: Para quaisquer duasvariáveis aleatórias x e y tem-se
Ex,y{x} = Ey{Ex{x|y}}
Usando a propriedade acima
Ex,y{‖x − x (y)‖2} = Ey{Ex{‖x − x (y)‖
2 |y}
Com o valor de y fixo, o problema de achar a função x (.) queminimiza o erro quadrático é equivalente a
minz∈RnEx{‖x − z‖2 |y} para todo y ∈ Rm
Controle Ótimo - Aula 5Filtro de Kalman – p.8/52
-
Estimativa Mínima Quadrática
Para cada z ∈ Rn fixo temos
Ex{‖x − z‖2 |y} = Ex{‖x‖
2 |y} − 2zT Ex{x|y} + ‖z‖2
Igualando a zero a derivada em relação a z, temos que a expressãoacima é minimizada por
z = Ex{x|y}
Custo ótimo
J∗ (z = Ex{x|y}) = Ex{‖x‖2 |y} − ‖Ex{x|y}‖
2
Controle Ótimo - Aula 5Filtro de Kalman – p.9/52
-
Estimativa Mínima Quadrática
Proposição E.2: Se x e y são vetores aleatórios conjuntamentegaussianos, então a estimativa mínima quadrática E{x|y} de x dado yé linear em y
Prova: Considere z =
[x
y
]e assuma que z é Gaussiano com média
z = E{z} =
[E{x}
E{y}
]=
[x
y
]e matriz de covariância
Σ = E{(z − z)(z − z)T}
=
[E{(x − x)(x − x)T} E{(x − x)(y − y)T}
E{(y − y)(x − x)T} E{(y − y)(y − y)T}
]=
[Σxx Σxy
Σyx Σyy
]
Controle Ótimo - Aula 5Filtro de Kalman – p.10/52
-
Estimativa Mínima Quadrática
Sendo z gaussiano, sua função densidade de probabilidade é
f (z) = f (x, y) = 1(2π)(n+m)/2(detΣ)1/2
exp[−12(z − z)T Σ−1 (z − z)]
De forma similar
f (x) = c1exp[−12(x − x)T Σ−1xx (x − x)]
f (y) = c2exp[−12(y − y)T Σ−1yy (y − y)]
Pela regra de Bayes
f (x|y) = f(x,y)f(y)
= c3exp[−1
2
((z − z)T Σ−1zz (z − z) − (y − y)
T Σ−1yy (y − y))]
Controle Ótimo - Aula 5Filtro de Kalman – p.11/52
-
Estimativa Mínima Quadrática
Note que Σ =
[Σxx Σxy
Σyx Σyy
]
=
[I ΣxyΣ
−1yy
0 I
][Σxx − ΣxyΣ
−1yy Σyx 0
0 Σyy
][I 0
Σ−1yy Σyx I
]
Assim
(z − z)T Σ−1 (z − z)
= (∗)T[(
Σxx − ΣxyΣ−1yy Σyx
)−10
0 Σ−1yy
][I −ΣxyΣ
−1yy
0 I
][x − x
y − y
]
= (∗)T[(
Σxx − ΣxyΣ−1yy Σyx
)−10
0 Σ−1yy
][x − x − ΣxyΣ
−1yy (y − y)
y − y
]
Controle Ótimo - Aula 5Filtro de Kalman – p.12/52
-
Estimativa Mínima Quadrática
= (∗)T(Σxx − ΣxyΣ
−1yy Σyx
)−1 (x − x − ΣxyΣ
−1yy (y − y)
)
+ (y − y)T Σ−1yy (y − y)
Portanto,((z − z)T Σ−1zz (z − z) − (y − y)
T Σ−1yy (y − y))
= (∗)T(Σxx − ΣxyΣ
−1yy Σyx
)−1 (x − x − ΣxyΣ
−1yy (y − y)
)
e
f (x|y) = f(x,y)f(y)
= c3exp[−1
2(∗)T
(Σxx − ΣxyΣ
−1yy Σyx
)−1 (x − x − ΣxyΣ
−1yy (y − y)
)]
Controle Ótimo - Aula 5Filtro de Kalman – p.13/52
-
Estimativa Mínima Quadrática
E assim, a função densidade de probabilidade condicional é gaussianade média(x + ΣxyΣ
−1yy (y − y)
)
e variância
Σxx − ΣxyΣ−1yy Σyx.
Segue que a média condicional de x dado y é dada por
Ex{x|y} =∫ +∞−∞
xf(x|y)dx = x + ΣxyΣ−1yy (y − y) = Ay + b
com
A = ΣxyΣ−1yy e b = x + ΣxyΣ
−1yy y
Controle Ótimo - Aula 5Filtro de Kalman – p.14/52
-
Estimativa Mínima QuadráticaLinear
Vamos considerar estimadores lineares
x (y) = Ay + b
Um estimador linear
x̂ (y) = Ây + b̂
sendo que  e b̂ minimizam
Ex,y{‖x − Ay − b‖2}
sobre todas as matrizes A ∈ Rn×m e vetores b ∈ Rn é denominado umestimador linear mínimo quadrático.
Controle Ótimo - Aula 5Filtro de Kalman – p.15/52
-
Estimativa Mínima QuadráticaLinear
Proposição E.3: Sejam duas variáveis aleatórias x e y, com dadadistribuição de probabilidade conjunta, e
E{x} = x, E{y} = y
E{(x − x)(x − x)T} = Σxx, E{(x − x)(y − y)T} = Σxy
E{(y − y)(x − x)T} = Σyx, E{(y − y)(y − y)T} = Σyy
O estimador linear mínimo quadrado de x dado y é dado por
x̂ (y) = x + ΣxyΣ−1yy (y − y)
A correspondente matriz de covariância do erro é dada por
Ex,y{(x − x̂ (y)) (x − x̂ (y))T} = Σxx − ΣxyΣ
−1yy Σyx
Controle Ótimo - Aula 5Filtro de Kalman – p.16/52
-
Estimativa Mínima QuadráticaLinear
Prova: O estimador linear é definido como
x̂ (y) = Ây + b̂
sendo que  e b̂ minimizam
f (A, b) = Ex,y{‖x − Ay − b‖2}
Derivando com relação a A e b e igualando a zero
∂f
∂A|Â,̂b
= 2Ex,y{(̂b + Ây − x)yT} = 0
∂f
∂b|Â,̂b
= 2Ex,y{b̂ + Ây − x} = 0 ⇒ b̂ = x − Ây
Controle Ótimo - Aula 5Filtro de Kalman – p.17/52
-
Estimativa Mínima QuadráticaLinear
Substituindo na primeira
Ex,y{y(Â(y − y) − (x − x))T} = 0
Tem-se que
Ex,y{Â(y − y) − (x − x)}T = 0
Então
yEx,y{Â(y − y) − (x − x)}T = 0
Fazendo
Ex,y{y(Â(y − y) − (x − x))T} − yEx,y{Â(y − y) − (x − x)}
T
= Ex,y{(y − y)(Â(y − y) − (x − x)T} = 0
Controle Ótimo - Aula 5Filtro de Kalman – p.18/52
-
Estimativa Mínima QuadráticaLinear
Ex,y{(y − y)(Â(y − y) − (x − x)T} = 0
é equivalente a
ΣyyÂT − Σyx = 0 ⇒ Â = Σ
TyxΣ
−1yy = ΣxyΣ
−1yy
Portanto
x̂ (y) = Ây + b̂ = x + ΣxyΣ−1yy (y − y)
A matriz de covariância do erro é obtida substituindo este resultado em
Ex,y{(x − x̂ (y)) (x − x̂ (y))T} = Σxx − ΣxyΣ
−1yy Σyx
Controle Ótimo - Aula 5Filtro de Kalman – p.19/52
-
Estimativa Mínima QuadráticaLinear
Note que as estimativas e matrizes de covariância do erro sãoexatamente iguais na Proposição E.2 e Proposição E.3.
Assim, em termos de desenvolvimento geral da teoria de estimativapodemos considerar que os ruídos são Gaussianos ou considerar que osruídos tem distribuição qualquer e considerar o melhor estimadorlinear.
Note que apenas no caso de ruídos são Gaussianos temos que a melhorestimativa linear x̂ (y) é igual a Ex{x|y}.
Quando voltarmos ao problema de controle ótimo com informaçãoimperfeita, vamos querer realimentar Ex{x|y} e assim, a hipótese deruídos Gaussianos é necessária.
Controle Ótimo - Aula 5Filtro de Kalman – p.20/52
-
Estimativa Mínima QuadráticaLinear
Corolário E.3.1: O estimador linear mínimo quadrático x̂ (y) énão-tendencioso (unbiased), isto é,
Ey{x̂ (y)} = x.
Prova:
Imediato substituindo a expressão de x̂ (y)
Ey{x̂ (y)} = Ey{x + ΣxyΣ−1yy (y − y)} = x
Controle Ótimo - Aula 5Filtro de Kalman – p.21/52
-
Estimativa Mínima QuadráticaLinear
Corolário E.3.2: O erro de estimativa x − x̂ (y) é não-correlacionadocom ambos, y e x̂ (y), isto é,
Ex,y{y(x − x̂ (y))T} = 0
Ex,y{x̂ (y) (x − x̂ (y))T} = 0
Prova: Primeira, equação da derivada de f em relação à A:Ex,y{(̂b + Ây − x)y
T} = 0.
Segunda, escrita como
Ex,y{(Ây + b̂)(x − x̂ (y))T}
= Ex,y{Ây(x − x̂ (y))T + b̂(x − x̂ (y))T} = 0
Controle Ótimo - Aula 5Filtro de Kalman – p.22/52
-
Estimativa Mínima QuadráticaLinear
Corolário E.3.3: Considere a variável aleatória z definida comocombinação linear de x
z = Cx
sendo C uma matriz p × n. Então a estimativa linear mínimoquadrático de z dado y é dado por
ẑ (y) = Cx̂ (y)
A correspondente matriz de covariância do erro é dada por
Ez,y{(z − ẑ (y)) (z − ẑ (y))T} = CEx,y{(x − x̂ (y)) (x − x̂ (y))
T}CT
Controle Ótimo - Aula 5Filtro de Kalman – p.23/52
-
Estimativa Mínima QuadráticaLinear
Prova: E{z} = z = Cx
Σzz = Ez{(z − z)(z − z)T} = CΣxxC
T
Σzy = Ez,y{(z − z)(y − y)T} = CΣxy
Pela Prop. 3.1ẑ (y) = z + ΣzyΣ
−1yy (y − y)
= Cx + CΣxyΣ−1yy (y − y) = Cx̂ (y)
Ez,y{(z − ẑ (y)) (z − ẑ (y))T} = Σzz − ΣzyΣ
−1yy Σyz
= C(Σxx − ΣxyΣ−1yy Σyx)C
T
= CEx,y{(x − x̂ (y)) (x − x̂ (y))T}CT
Controle Ótimo - Aula 5Filtro de Kalman – p.24/52
-
Estimativa Mínima QuadráticaLinear
Corolário E.3.4: Considere x e y de média e covariância conjunta(conhecidas) e defina a variável aleatória z como combinação linearde y
z = Cy + u
sendo C uma matriz p × m de posto linha pleno e u um vetor p × 1dado. Então o estimador linear mínimo quadrático de x dado z é dadopor
x̂ (z) = x + ΣxyCT
(CΣyyC
T)−1
(z − Cy − u)
e a correspondente matriz de covariância do erro é dada por
Ex,z{(x − x̂ (z)) (x − x̂ (z))T} = Σxx − ΣxyC
T(CΣyyC
T)−1
CΣyx
Controle Ótimo - Aula 5Filtro de Kalman – p.25/52
-
Estimativa Mínima QuadráticaLinear
Prova: E{z} = z = Cy + u
Σzz = Ez{(z − z)(z − z)T} = CΣyyC
T
Σzx = Ez,y{(z − z)(x − x)T} = CΣyx
Pela Prop. 3.1x̂ (z) = x + ΣxzΣ
−1zz (z − z)
= x + ΣxyCT
(CΣyyC
T)−1
(z − Cy − u)
Ez,x{(x − x̂ (z)) (x − x̂ (z))T} = Σxx − ΣxzΣ
−1zz Σzx
= Σxx − ΣxyCT
(CΣyyC
T)−1
CΣyx
Controle Ótimo - Aula 5Filtro de Kalman – p.26/52
-
Estimativa Mínima QuadráticaLinear
Corolário E.3.5: Seja
z = Cx + v
sendo C uma matriz m × n e v um vetor m × 1 dado. Considere que xe v são não correlacionados. Então o estimador linear mínimoquadrático de x dado z é dado por
x̂ (z) = x + ΣxxCT
(CΣxxC
T − Σvv)−1
(z − Cx − v)
e a correspondente matriz de covariância do erro é dada por
Ex,v{(x − x̂ (z)) (x − x̂ (z))T} = Σxx − ΣxxC
T(CΣxxC
T)−1
CΣxx
Controle Ótimo - Aula 5Filtro de Kalman – p.27/52
-
Estimativa Mínima QuadráticaLinear
Prova: Sejam y = (xT vT )T , y = (xT vT )T
Temos x = (I 0)y e pelo Cor. E.3.3
x̂(z) = (I 0)ŷ(z)
E{(x − x̂ (z)) (x − x̂ (z))T} =
(I 0)E{(y − ŷ (z)) (y − ŷ (z))T}(I 0)T
Temos ainda z = C̃y, sendo C̃ = (C I).
Controle Ótimo - Aula 5Filtro de Kalman – p.28/52
-
Estimativa Mínima QuadráticaLinear
Utilizando o Cor. E.3.4 com u = 0 e x = y temos
ŷ (z) = y + ΣyyC̃T
(C̃ΣyyC̃
T)−1 (
z − C̃y)
E{(y − ŷ (z)) (y − ŷ (z))T} = Σyy − ΣyyC̃T
(C̃ΣyyC̃
T)−1
C̃Σyy
Substituindo
Σyy =
[Σxx 0
0 Σvv
]e C̃ = (C I)
o resultado é obtido
Controle Ótimo - Aula 5Filtro de Kalman – p.29/52
-
Estimativa Mínima QuadráticaLinear
Corolário E.3.6:Considere x, y e z vetores aleatórios de média ecovariância conhecidas com y e z não correlacionados e com Σzz > 0
Então o estimador linear mínimo quadrático de x dados y e z seescreve em termos dos estimadores individuais como
x̂ (y, z) = x̂ (y) + x̂ (z) − x
e a correspondente matriz de covariância do erro é
Ex,y,z{(x − x̂ (y, z)) (x − x̂ (y, z))T}
= Σxx − ΣxyΣ−1yy Σyx − ΣxzΣ
−1zz Σzx
Controle Ótimo - Aula 5Filtro de Kalman – p.30/52
-
Estimativa Mínima QuadráticaLinear
Prova: Sejam w = (yT zT )T , w = (yT zT )T
Pela Prop. E.3
x̂ (w) = x + ΣxwΣ−1ww (w − w)
Temos
Σxw =[E{(x − x)(y − y)T} E{(x − x)(z − z)T}
]
=[Σxy Σxz
]
e, como y e z são não correlacionados
Σww =
[Σyy 0
0 Σzz
]
Controle Ótimo - Aula 5Filtro de Kalman – p.31/52
-
Estimativa Mínima QuadráticaLinear
Substituindo e somando e subtraindo x
x̂ (y, z) = x + ΣxyΣ−1yy (y − y) + x + ΣxzΣ
−1zz (z − z) − x =
x̂ (y) + x̂ (z) − x
Pela Prop. E.3, temos que a correspondente matriz de covariância doerro é dada por
Ex,y,z{(x − x̂ (y, z)) (x − x̂ (y, z))T} = Σxx − ΣxwΣ
−1wwΣwx
= Σxx −[Σxy Σxz
] [Σyy 00 Σzz
]−1 [Σyx
Σzx
]
= Σxx − ΣxyΣ−1yy Σyx − ΣxzΣ
−1zz Σzx
Controle Ótimo - Aula 5Filtro de Kalman – p.32/52
-
Estimativa Mínima QuadráticaLinear
Corolário E.3.7:Considere x, y e z como anteriormente, mas com y ez não necessariamente não correlacionados. Então o estimador linearmínimo quadrático de x dados y e z é
x̂ (y, z) = x̂ (y) + x̂ (z − ẑ (y)) − x
e a correspondente matriz de covariância do erro é
Ex,y,z{(x − x̂ (y, z)) (x − x̂ (y, z))T}
= Σxx − ΣxyΣ−1yy Σyx − Σ̂xzΣ̂
−1zz Σ̂zx
sendoΣ̂xz = E{(x − x) (z − ẑ (y))
T}
Σ̂zz = E{(z − ẑ (y)) (z − ẑ (y))T}
Controle Ótimo - Aula 5Filtro de Kalman – p.33/52
-
Estimativa Mínima QuadráticaLinear
Prova: Como ẑ (y) é uma função linear em y, o estimador x dado(y, z) é o mesmo estimador de x dado (y, z − ẑ (y)).
x (y, z − ẑ (y)) = A
[y
z − ẑ (y)
]+ b =
[A1 A2
] [ yz − αy − β
]+ b
= A1y + A2 (z − αy − β) + b
= (A1 − A2α) y + A2z + (b − A2β) = x (y, z)
Pelo Corol. E.3.2, os vetores aleatórios y e z − ẑ (y) são nãocorrelacionados
Assim, o resultado segue do Corol. E.3.6.
Controle Ótimo - Aula 5Filtro de Kalman – p.34/52
-
Estimativa Mínima Quadrática dosestados
Considere o seguinte sistema linear
xk+1 = Akxk + wk, k = 0, .., N − 1
sendo xk ∈ Rn o estado, wk ∈ Rn o distúrbio aleatório, e as matrizesAk ∈ R
n×n são conhecidas.
Vamos assumir que o estado xk não é disponível, mas que temos a cadainstante a seguinte medida do estado
zk = Ckxk + vk, k = 0, .., N − 1
sendo zk ∈ Rs o vetor das variáveis observadas, vk ∈ Rs o ruídoaleatório de medida.
Controle Ótimo - Aula 5Filtro de Kalman – p.35/52
-
Estimativa Mínima Quadrática dosestados
Para os vetores aleatórios independentes, x0, wk, vk, assumimos que
H1) a condição inicial é gaussiana com covariância S :
E{(x0 − E{x0}) (x0 − E{x0})T} = S
H2) o distúrbio wk é gaussiano com média zero e branco:
E{wk} = 0, E{wkwTk } = Mk ≥ 0
H3) o distúrbio vk é gaussiano com média zero e branco:
E{vk} = 0, E{vkvTk } = Nk > 0
Controle Ótimo - Aula 5Filtro de Kalman – p.36/52
-
Estimativa Mínima Quadrática dosestados: forma não recursiva
Baseados nos resultados de estimativa mínimos quadrados, podemosdeterminar facilmente uma expressão para a estimativa de xk ou aindade xk+1 dados os valores de zk, zk−1, ..., z0.
Considere as informações medidas e os ruídos na forma matricial
Zk =
z0...
zk−1
zk
, rk−1 =
x0
w0...
wk−1
, Vk =
v0
v1...
vk
Controle Ótimo - Aula 5Filtro de Kalman – p.37/52
-
Estimativa Mínima Quadrática dosestados: forma não recursiva
Neste caso, o sistema de equações de estado se escreve como
xi+1 = Liri, 0 ≤ i ≤ k
sendo Li a matriz n × (n (i + 1)) dada por
Li =[Ai · · ·A0, Ai · · ·A1, · · · , Ai, I
]
Controle Ótimo - Aula 5Filtro de Kalman – p.38/52
-
Estimativa Mínima Quadrática dosestados: forma não recursiva
Assim temos,
Zk = Φk−1rk−1 + Vk
sendo Φk−1 a matriz s (k + 1) × (nk) dada por
Φk−1 =
C0 0 · · · · · · 0
C1L0 0 · · · 0...
. . ....
Ck−1Lk−2 0
CkLk−1
Controle Ótimo - Aula 5Filtro de Kalman – p.39/52
-
Estimativa Mínima Quadrática dosestados: forma não recursiva
Temos xk expresso como uma função linear em rk−1. Assim, oproblema está essencialmente resolvido se obtivermos uma estimativamínimo quadrado para rk−1 dado Zk usando (2).
Notação:
x̂k+1|k = x̂k+1 (Zk): estimativa linear mínimo quadrática de xk+1 dadoZk
x̂k|k = x̂k (Zk) estimativa mínimo quadrática de xk dado Zk
r̂k−1|k = r̂k−1 (Zk) estimativa mínimo quadrática de rk−1dado Zk
Controle Ótimo - Aula 5Filtro de Kalman – p.40/52
-
Estimativa Mínima Quadrática dosestados: forma não recursiva
Pelo Corolário E.3.5 podemos determinar
r̂k−1|k e E{(rk−1 − r̂k−1|k
) (rk−1 − r̂k−1|k
)T}
e pelo Corolário E.3.3 temos
x̂k|k = Lk−1r̂k−1|k
E{(xk − x̂k|k
) (xk − x̂k|k
)T} =
Lk−1E{(rk−1 − r̂k−1|k
) (rk−1 − r̂k−1|k
)T}LTk−1
Note que quando o número de medidas k se torna grande esteprocedimento se torna muito trabalhoso computacionalmente.
Controle Ótimo - Aula 5Filtro de Kalman – p.41/52
-
Filtro de Kalman
Filtro de Kalman: Estimador mínimo quadrático dos estados para umsistema dinâmico de tempo discreto na forma recursiva
Rudolf Emil Kalman
(Budapeste, Hungria, 1930)
R. E. Kalman. A new approach to linear filtering and predictionproblems. Transactions of the ASME-Journal of Basic Engineering,vol. 82, pp.35-45, 1960.
Controle Ótimo - Aula 5Filtro de Kalman – p.42/52
-
Filtro de Kalman
Notação:x̂k+1|k = x̂k+1 (Zk): estimativa linear mínimo quadrática de xk+1 dadoZk
x̂k|k = x̂k (Zk) estimativa mínimo quadrática de xk dado Zk
Σk+1|k: matriz covariância do erro para estimar xk+1 dado Zk, ou seja,
Σk+1|k = E{(xk+1 − x̂k+1|k
) (xk+1 − x̂k+1|k
)T}
Σk|k: matriz covariância do erro para estimar xk dado Zk, ou seja,
Σk|k = E{(xk − x̂k|k
) (xk − x̂k|k
)T}
Controle Ótimo - Aula 5Filtro de Kalman – p.43/52
-
Filtro de Kalman
Suponha que x̂k|k−1 já tenha sido calculado e
Σk|k−1 = E{(xk − x̂k|k−1
) (xk − x̂k|k−1
)T}
No tempo k, obtém-se a medida
zk = Ckxk + vk
Pelo Corol. E.3.7, calcula-se o estimador linear de xk dadosZk−1 = (z
T0 , ..., z
Tk−1) e zk:
x̂k|k = x̂k|k−1 + x̂k(zk − ẑk(Zk−1)) − E{xk}
sendo ẑk(Zk−1) o estimador linear de zk dado Zk−1 ex̂k(zk − ẑk(Zk−1)) o estimador linear de xk dado zk − ẑk(Zk−1)
Controle Ótimo - Aula 5Filtro de Kalman – p.44/52
-
Filtro de Kalman
Cálculo de x̂k(zk − ẑk(Zk−1)). Pelo Corol. E.3.3:
ẑk(Zk−1) = Ckx̂k|k−1
e E{(zk − ẑk(Zk−1)) (zk − ẑk(Zk−1))T} = CkΣk|k−1C
Tk + Nk
Temos
E{xk (zk − ẑk(Zk−1))T} = E{xk
(Ck(xk − x̂k|k−1)
)T} + E{xkv
Tk }
=E{(xk − x̂k|k−1)(xk − x̂k|k−1)T}CTk + E{x̂k|k−1(xk − x̂k|k−1)T}CTk
=E{(xk − x̂k|k−1)(xk − x̂k|k−1)T}CTk = Σk|k−1CTk
pois E{x̂k|k−1(xk − x̂k|k−1)T}CTk = 0 pelo Corl. E.3.2
Controle Ótimo - Aula 5Filtro de Kalman – p.45/52
-
Filtro de Kalman
Utilizando as equações anteriores e a Prop. E.3
x̂k(zk − ẑk(Zk−1)) =
E{xk} + Σk|k−1CTk
(CkΣk|k−1C
Tk + Nk
)−1(zk − Ckx̂k|k−1)
Portanto,
x̂k|k = x̂k|k−1 + x̂k(zk − ẑk(Zk−1)) − E{xk}
= x̂k|k−1 + Σk|k−1CTk
(CkΣk|k−1C
Tk + Nk
)−1(zk − Ckx̂k|k−1)
Pelo Corol. E.3.3
x̂k+1|k = Akx̂k|k
Controle Ótimo - Aula 5Filtro de Kalman – p.46/52
-
Filtro de Kalman
Considerando a equação dinâmica, a hipótese H2 e o Corol. E.3.3
E{(xk+1 − x̂k+1|k
) (xk+1 − x̂k+1|k
)T}
= AkE{(xk − x̂k|k
) (xk − x̂k|k
)T}ATk + Mk
Σk+1|k = AkΣk|kATk + Mk
A matriz de covariância Σk|k pode ser calculada pelo Corol. E.3.7 deforma similar ao realizado para x̂k|k
Σk|k = Σk|k−1 − Σk|k−1CTk
(CkΣk|k−1C
Tk + Nk
)−1CkΣk|k−1
Controle Ótimo - Aula 5Filtro de Kalman – p.47/52
-
Filtro de Kalman
Portanto,x̂k|k = x̂k|k−1 + Σk|k−1C
Tk
(CkΣk|k−1C
Tk + Nk
)−1(zk − Ckx̂k|k−1)
x̂k+1|k = Akx̂k|k
Σk|k = Σk|k−1 − Σk|k−1CTk
(CkΣk|k−1C
Tk + Nk
)−1CkΣk|k−1
Σk+1|k = AkΣk|kATk + Mk
Constituem o Filtro de Kalman, com condições iniciais x̂0|−1 = E{x0}e Σ0|−1 = S
Controle Ótimo - Aula 5Filtro de Kalman – p.48/52
-
Filtro de Kalman
Σk|kCTk N
−1k =(
Σk|k−1 − Σk|k−1CTk
(CkΣk|k−1C
Tk + Nk
)−1CkΣk|k−1
)CTk N
−1k
= Σk|k−1CTk
(N−1k −
(CkΣk|k−1C
Tk + Nk
)−1CkΣk|k−1C
Tk N
−1k
)
Como N−1k =(CkΣk|k−1C
Tk + Nk
)−1 (CkΣk|k−1C
Tk + Nk
)N−1k
=(CkΣk|k−1C
Tk + Nk
)−1 (CkΣk|k−1C
Tk N
−1k + I
)
Temos
Σk|kCTk N
−1k = Σk|k−1C
Tk
(CkΣk|k−1C
Tk + Nk
)−1
Controle Ótimo - Aula 5Filtro de Kalman – p.49/52
-
Filtro de Kalman
Forma alternativa
x̂k|k = Ak−1x̂k−1|k−1 + Σk|kCTk N
−1k (zk − CkAk−1x̂k−1|k−1)
Se a equação dinâmica contém o controle uk
xk+1 = Akxk + wk + uk, k = 0, .., N − 1
Temos
x̂k|k = Ak−1x̂k−1|k−1 + Bk−1uk−1 + Σk|kCTk N
−1k (zk −
CkAk−1x̂k−1|k−1 − CkBk−1uk−1)
sendo x̂k|k o estimador linear mínimo qudrático de xk dados z0, ..., zk eu0, ..., uk−1. as equações que geram Σk|k são as mesmas
Controle Ótimo - Aula 5Filtro de Kalman – p.50/52
-
Filtro de Kalman: regimepermanente
Para condição inicial Σ0|−1 = S, temos
Σk+1|k =
Ak
(Σk|k−1 − Σk|k−1C
Tk
(CkΣk|k−1C
Tk + Nk
)−1CkΣk|k−1
)ATk + Mk
Esta é uma Equação de Riccati.
Considerando as matrizes do sistema constantes (Ak = A,...), o par(A,C) observável e o par (A,D) controlável, sendo M = DT D, então
Σ = A(Σ − ΣCT
(CΣCT + N
)−1CΣ
)AT + M
Controle Ótimo - Aula 5Filtro de Kalman – p.51/52
-
Filtro de Kalman: regimepermanente
Nas mesmas condições,Σk|k → Σ
Σ = Σ − ΣCT(CΣCT + N
)−1CΣ
Portanto
x̂k|k = Ax̂k−1|k−1 + Buk−1 + ΣCT N−1(zk − CAx̂k−1|k−1 − CBuk−1)
Controle Ótimo - Aula 5Filtro de Kalman – p.52/52
large {Informação imperfeita}large {Estabelecimento do problema}large {Estabelecimento do problema}large {Estabelecimento do problema}large {Estimativa Ma 'i nima Quadrática}large {Estimativa Ma 'i nima Quadrática}large {Estimativa Ma 'i nima Quadrática}large {Estimativa Ma 'i nima Quadrática}large {Estimativa Ma 'i nima Quadrática}large {Estimativa Ma 'i nima Quadrática}large {Estimativa Ma 'i nima Quadrática}large {Estimativa Ma 'i nima Quadrática}large {Estimativa Ma 'i nima Quadrática}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática Linear}large {Estimativa Ma 'i nima Quadrática dos estados}large {Estimativa Ma 'i nima Quadrática dos estados}large {Estimativa Ma 'i nima Quadrática dos estados: forma não recursiva}large {Estimativa Ma 'i nima Quadrática dos estados: forma não recursiva}large {Estimativa Ma 'i nima Quadrática dos estados: forma não recursiva}large {Estimativa Ma 'i nima Quadrática dos estados: forma não recursiva}large {Estimativa Ma 'i nima Quadrática dos estados: forma não recursiva}large {Filtro de Kalman}large {Filtro de Kalman}large {Filtro de Kalman}large {Filtro de Kalman}large {Filtro de Kalman}large {Filtro de Kalman}large {Filtro de Kalman}large {Filtro de Kalman}large {Filtro de Kalman}large {Filtro de Kalman: regime permanente}large {Filtro de Kalman: regime permanente}