FILTRO DE KALMAN
Paulo Lopes dos Santos
Departamento de Engenharia Electrotécnica e Computadores
Faculdade de Engenharia da Universidade do Porto
Rua Dr Roberto Frias, s/n
4200-464 Porto, Portugal
Email: pjsantos@fe.up.pt
Novembro 2012
Filtro de Kalman
1
Conteúdo
1 Introdução
2
2 Observadores determinı́sticos
2
2.1
Abordagem 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
2.2
Abordagem 2 - O observador de Luenberger . . . . . . . . . . . . . . . . . .
6
2.2.1
6
Sistemas sem perturbações . . . . . . . . . . . . . . . . . . . . . . . .
3 Estimação de estado de sistemas com perturbações
9
3.1
Introdução . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
3.2
Observação de estados em sistemas com perturbações determinı́sticas . . . .
10
3.2.1
Modelo Interno dum degrau . . . . . . . . . . . . . . . . . . . . . . .
12
Modelo Interno da rampa . . . . . . . . . . . . . . . . . . . . . . . . . . . .
12
3.3.1
Modelo interno duma sinusoide com frequência ω0 . . . . . . . . . . .
12
3.4
Estimação duma perturbação em degrau . . . . . . . . . . . . . . . . . . . .
12
3.5
Observação de estados em sistemas com perturbações estocásticas . . . . . .
14
3.6
Estimação de estado em modelos de estado determinı́stico-estocásticos . . . .
15
3.7
Previsor de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
3.8
Previsor de Kalman estacionário e modelo de inovação . . . . . . . . . . . .
24
3.3
4 Filtro de Kalman
30
5 Filtro de Kalman de sistemas perturbados ruı́dos de estado e de medição
não correlacionados
6 Sintonização do Previsor e do Filtro de Kalman
37
38
6.1
Sistemas estáveis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
38
6.2
Sistemas instáveis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
6.2.1
Passeio aleatório . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
6.2.2
Integrador com constante de tempo . . . . . . . . . . . . . . . . . . .
53
6.2.3
Sistemas com dois ou mais Integradores . . . . . . . . . . . . . . . . .
62
A - Matrizes (semi)definidas positivas e negativas
73
Filtro de Kalman
1
2
Introdução
O estado dum sistema é um conjunto de variáveis que contém a sua memória. Uma representação no espaço de estados é um modelo matemático dum sistema dinâmico que relaciona
as suas variáveis de estado com as suas entradas e saı́das através duma equação diferencial
(no caso de sistemas contı́nuos) ou às diferenças (no caso de sistemas discretos) de primeira
ordem. No caso de sistemas discretos, lineares e invariantes no tempo, estas representações
são do tipo
x(t + 1) = Ax(t) + Bu(t)
y(t) = Cx(t) + Du(t).
(1)
onde y(t) ∈ IRℓ é a saı́da, u(t) ∈ IRm a entrada e x(t) ∈ IRn o vetor das variáveis de estado.
As matrizes A ∈ IRn×n , B ∈ IRn×m , C ∈ IRℓ×n , e D ∈ IRℓ×m são os parâmetros do sistema.
O conhecimento das variáveis de estado x(t) é importante para resolver muitos problemas
tais como a estabilização de sistemas através da realimentação de estado, a monitorização,
deteção de avarias, etc.. No entanto, nem sempre é possı́vel medir diretamente as variáveis
de estado através de sensores porque ou esses sensores não existem, ou são demasiado caros.
Para resolver este problema, procura-se estimar o valor do estado através da observação dos
sinais de entrada e de saı́da do sistema, utilizando um observador (ou estimador) de estado.
2
2.1
Observadores determinı́sticos
Abordagem 1
Um observador de estado é um sistema dinâmico que, a partir dos sinais de entrada e saı́da,
fornece estimativas do estado dum determinado sistema. É utlizado para:
• Estabilização de sistemas utilizando a realimentação de estado,
• monitorização de sistemas,
• soft sensor para substituir sensores de custo elevado,
• Detecção de avarias.
Filtro de Kalman
3
Numa primeira abordagem vamos considerar um observador com um modelo exatamente
igual ao do sistema que pretendemos observar, ou seja,
x̂(t + 1) = Ax̂(t) + Bu(t)
ŷ(t) = C x̂(t) + Du(t).
(2)
Definindo
x̃(t) = x(t) − x̂(t)
(3)
como erro do observador, podemos analisar o seu comportamento subtraindo as equações (2)
do observador às equações (1) do sistema:
x(t + 1) − x̂(t + 1) = Ax(t) + Bu(t) − Ax̂(t) − Bu(t) = Ax(t) − Ax̂(t) = A [x(t) − x̂(t)] . (4)
Substituindo x(·) − x̂(·) por x̃(·) obtemos
x̃(t + 1) = Ax̃(t).
(5)
Esta equação é a dum sistema autónomo1 linear e invariante no tempo com a matriz de
estado igual à do sistema observado. O erro anula-se se e só se o sistema for estável. Logo,
este observador não consegue observar sistemas instáveis. Por outro lado, a dinâmica de
anulação do erro é imposta pelo sistema não podendo ser alterada. O exemplo seguinte
ilustra o desempenho dum observador deste tipo na estimação do vetor de estados dum
sistema instável.
Exemplo 1 (Motor de corrente contı́nua):
Consideremos o motor elétrico de corrente contı́nua descrito pelo modelo amostrado
x1 (t + 1) = x1 (t) + x2 (t)
x2 (t + 1) = 0.9x2 (t) + 10u(t)
y(t) = x1 (t)
onde o perı́odo de amostragem é Ts = 1 segundo e
• x1 é a posição angular do veio,
• x2 a velocidade de rotação,
1
Sistema autónomo é um sistema sem entradas
(6)
4
Filtro de Kalman
• u a tensão de entrada.
Reescrevendo as equações (6) na forma matricial teremos
x(t + 1) = Ax(t) + Bu(t)
y(t) = Cx(t) + Du(t)
com
x(t) =
x1 (t)
x2 (t)
,
A=
1 1
0 0.9
,
B=
0
10
,
C=
Os valores próprios de A são
λA1 = 1,
1 0 ,
D = 0.
(7)
λA2 = 0.9
pelo que o observador (2) é um sistema instável e, consequentemente, não funciona convenientemente porque o erro x̃(t) não converge para zero.
Para ilustrar o desempenho deste observador o sistema foi excitado pela sequência binária
y(t) = x1 (t)
3000
2000
1000
0
−1000
0
200
400
600
800
1000
600
800
1000
600
800
1000
Tempo
x2 (t)
100
50
0
−50
−100
0
200
400
Tempo
u(t)
1
0.5
0
−0.5
−1
0
200
400
Tempo
Figura 1: Sinais do sistema: y(t) = x1 (t) posição do veio, x1 (t) velocidade angular, u(t)
tensão de entrada (sequência binária aleatória).
aleatória apresentada na figura 1 juntamente a resposta do sistema (posição e velocidade).
A figuras 2-4 comparam as estimativas do observador com as variáveis de estado do sistema.
5
Filtro de Kalman
y(t) = x1 (t)
3000
2000
1000
0
−1000
0
200
400
600
800
1000
600
800
1000
Tempo
x2 (t)
100
50
0
−50
−100
0
200
400
Tempo
Figura 2: Variáveis de estado (azul) e estimativas do observador (a vermelho)
Podemos ver que, por ser assintoticamente instável, este observador apenas conseguiu estimar a posição x1 (t) a menos duma constante com um valor muito significativo (cerca de
500). Por outro lado vemos nas figuras 3 e 4 que o observador responde com uma constante
x̃1 (t) = x1 (t) − x̂1 (t)
1000
500
0
−500
0
200
400
600
800
1000
600
800
1000
Tempo
x̃2 (t) = x2 (t) − x̂2 (t)
100
80
60
40
20
0
0
200
400
Tempo
Figura 3: Erros do observador
de tempo igual à do motor, ou seja, 9.5 segundos (cerca de 10 segundos).
6
Filtro de Kalman
x̃1 (t) = x1 (t) − x̂1 (t)
500
0
−500
0
20
40
x̃2 (t) = x2 (t) − x̂2 (t)
60
Tempo
80
100
60
80
100
100
80
60
40
20
0
0
20
40
Tempo
Figura 4: Erros do observador (ampliação dos instantes iniciais).
2.2
2.2.1
Abordagem 2 - O observador de Luenberger
Sistemas sem perturbações
Se, na equação de estado do observador (2), adicionarmos um termo corretivo proporcional
ao erro da estimativa da saı́da, obtemos um observador de Luenberger. Este observador é
então descrito pelo modelo
x̂(t + 1) = Ax̂(t) + Bu(t) + L [y(t) − C x̂(t) − Du(t)]
ŷ(t) = C x̂(t) + Du(t)
(8)
onde L ∈ IRn×ℓ é o chamado ganho do observador. Tal como no caso anterior, o desempenho
deste observador é analisado através da dinâmica do erro x̃(t) definido em (3). Assim,
subtraindo a equação de estado de (8) à de (1), temos
x(t + 1) − x̂(t + 1) = Ax(t) + Bu(t) − Ax̂(t) − Bu(t) − L [y(t) − C x̂(t) − Du(t)] =
= Ax(t) − Ax̂(t) − L [Cx(t) + Du(t) − C x̂(y) − Du(t)] =
(9)
= A [x(t) − x̂(t)] − LC [x(t) − x̂(t)] = (A − LC) [x(t) − x̂(t)] ,
e, novamente, Substituindo x(·) − x̂(·) por x̃(·), obtemos
x̃(t + 1) = (A − LC) x̃(t).
(10)
A dinâmica do erro do observador é novamente descrita por um sistema autónomo linear
mas agora com uma matriz de estado diferente da do sistema. Se o par (A, C) for observável,
7
Filtro de Kalman
os valores próprios do observador podem ser alocados arbitrariamente através dum cálculo
adequado do ganho do observador. Deste modo, dinâmica do observador não é imposta pelo
sistema e, mesmo que este seja instável, é possı́vel construir um observador assintóticamente
estável, isto é, um observador cujo erro tenda para zero quando o tempo tende para infinito.
A única condição para que isto seja possı́vel é que os modos instáveis sejam observáveis.
Exemplo 2:
Consideremos novamente o sistema (6). Pretendemos projectar um observador de Luenberger que tenha λ1 e λ2 como valores próprios. Temos então que calcular
ℓ
L= 1
ℓ2
(11)
por forma a que
Λ (A − LC) = (λ1 , λ2 )
em que Λ (M) significa “o conjunto dos valores próprios de M”. Teremos, então,
det [λI2 − (A − LC)] = (λ − λ1 ) (λ − λ2 ) = λ2 − (λ1 + λ2 ) λ + λ1 λ2 .
Para A e C definidos em (7) e L definido em (11), esta equação será
λ − (1 − ℓ1 )
−1
det [λI2 − (A − LC)] = det
ℓ2
λ − 0.9
= λ2 − (1.9 − ℓ1 ) λ + (1 − ℓ1 ) 0.9 + ℓ2 = λ2 − (1.9 − ℓ1 ) λ + (1 − ℓ1 ) 0.9 + ℓ2 ,
de onde podemos escrever o sistema de equações linear que nos permite determinar ℓ1 e ℓ2 :
1.9 − ℓ1 = λ1 + λ2
ℓ1 = 1.9 − (λ1 + λ2 )
⇒
(1 − ℓ1 ) 0.9 + ℓ2 = λ1 λ2
ℓ2 = 0.81 − 0.9 (λ1 + λ2 ) + λ1 λ2
Foram projetados 4 observadores, todos com um valor próprio duplo, tomando valores de 0 a
Tabela 1: Valores próprios e ganhos do observadores projetados
Observador
Observador
Observador
Observador
1
2
3
4
λ1
0.8100
0.6000
0.3500
0.0000
λ2
0.8100
0.6000
0.3500
0.0000
ℓ1
0.2800
0.7000
1.2000
1.9000
ℓ2
0.0081
0.0900
0.3025
0.8100
8
Filtro de Kalman
Valor verdadeiro
Observador 1
Observador 2
Observador 3
Observador 4
x1 (t) = y(t)
3000
2000
1000
0
−1000
0
200
400
600
800
1000
600
800
1000
Tempo
x1 (t) = y(t)
200
0
−200
−400
−600
0
200
400
Tempo
Figura 5: Variáveis de estado (azul) e estimativas dos observadores 1, 2, 3, e 4 (praticamente
indistinguı́veis) quando não há perturbações.
0.81 (ver Tabela 1). A figuras 5-8 comparam as estimativas dos diferentes observadores com
as variáveis de estado do sistema. Todas as estimativas convergem para o valor verdadeiro
do estado. Na figura 8 pode-se ver claramente que quanto menores forem os valores próprios
mais rápido será o observador. Por outro lado o valor máximo do erro parece ser maior
Valor verdadeiro
Observador 1
Observador 2
Observador 3
Observador 4
x1 (t) = y(t)
1000
500
0
−500
−1000
0
10
20
30
40
50
30
40
50
Tempo
x2 (t)
200
0
−200
−400
−600
0
10
20
Tempo
Figura 6: Ampliação dos instantes iniciais da evolução das variáveis de estado (azul) e das
estimativas dos observadores 1, 2, 3 e 4 quando não há perturbações.
quanto menores forem os valores próprios.
9
Filtro de Kalman
Observador 1
Observador 2
Observador 3
Observador 4
x̃1 (t) = x1 (t) − x̂1 (t)
1000
500
0
−500
0
200
400
Tempo
600
800
1000
600
800
1000
x̃2 (t) = x2 (t) − x̂2 (t)
600
400
200
0
−200
0
200
400
Tempo
Figura 7: Erros dos observadores 1, 2, 3 e 4 quando não há perturbações.
Observador 1
Observador 2
Observador 3
Observador 4
x̃1 (t) = x1 (t) − x̂1 (t)
1000
500
0
−500
0
10
20
Tempo
30
40
50
30
40
50
x̃2 (t) = x2 (t) − x̂2 (t)
600
400
200
0
−200
0
10
20
Tempo
Figura 8: Ampliação dos instantes iniciais dos erros dos observadores 1, 2, 3 e 4 quando não
há perturbações.
3
3.1
Estimação de estado de sistemas com perturbações
Introdução
No exemplo anterior todos os observadores tiveram um comportamento satisfatório na medida em que o erro de estimação anulou-se ao fim de algum tempo. No entanto, isto aconteceu
porque o sistema observado não esteve sujeito a perturbações. As perturbações são sinais
desconhecidos que podem entrar no sistema através da equação de estado ou da saı́da. Normalmente consideram-se perturbações do tipo aditivo sendo um sistema com perturbações
10
Filtro de Kalman
descrito pelo modelo
x(t + 1) = Ax(t) + Bu(t) + q(t)
y(t) = Cx(t) + r(t)
(12)
onde q(t) ∈ IRn , o ruı́do de estado e r(t) ∈ IRℓ , o ruı́do de medição, são as perturbações.
Subtraindo a equação de estado do observador, equação (8), à deste modelo obtemos a
equação que descreve a dinâmica do erro de estimação e que é dada por
x̃(t + 1) = (A − LC)x̃(t) + q(t) − Lr(t).
(13)
Podemos ver que o erro passou a ser o estado dum sistema excitado por q(t) e q(t). Se estes
sinais não forem ambos nulos o erro nunca poderá ser nulo. Nestas condições, o ganho do
observador deverá tornar o erro o mais pequeno possı́vel. Nesta perspetiva, o cálculo do
ganho do observador pode ser efetuado através da minimização dum critério do erro, como
por exemplo, o critério quadrático
V2 =
N
X
kx̃k22 =
t=1
N
X
x̃T (t)x̃(t),
t=1
o critério minmax
Vm = min {max x̃(t)} ,
etc.. No entanto, como o erro não é conhecido, é sempre necessário assumir algumas asserções sobre as perturbações q(t) e r(t) para que qualquer critério seja minimizado. No
caso determinı́stico, assume-se que a perturbação é um determinado sinal, tal como uma
degrau, uma rampa, uma sinusoide, etc, e, utilizando o modelo interno desse sinal, altera-se
o observador por forma a que o erro de estimação seja nulo em regime permanente. No caso
estocástico a asserção mais frequente é considerar que estas perturbações são ruı́do branco.
3.2
Observação de estados em sistemas com perturbações determinı́sticas
Se q(t) for um sinal determinı́stico pode ser visto como a saı́da dum sistema linear e invariante
excitado por um impulso de Dirac. Tratando-se dum sistema linear e invariante no tempo,
11
Filtro de Kalman
Uq (z) = 1
Q(z)
Q(z)
Figura 9: Modelo interno do sinal determinı́stico.
pode ser descrito pela função de transferência Q(z), igual à transformada z do sinal, ou por
um modelo no espaço de estados
xq (t + 1) = Aq xq (t) + Bq (t)uq (t)
(14)
q(t) = Cq (q)xq (t)
com xq (t) ∈ Rnq e uq (t) ∈ R um impulso de Dirac tal que
Q(z) = Cq zInq − Aq
−1
Bq .
(15)
de forma idêntica, a perturbação r(t) também pode ser vista como a saı́da do sistema linear
e invariante no tempo com modelo e estado
xr (t + 1) = Ar xr (t) + Br (t)ur (t)
(16)
r(t) = Cr (q)xr (t)
com xr (t) ∈ Rnr , ur (t) ∈ R também um impulso de Dirac e
R(z) = Cr (zInr − Ar )−1 Br .
(17)
Substituindo em (12) q(t) e r(i) pelos respetivos modelos internos (14) e (16), obtem-se o
modelo alargado

 




xq (t)
xq (t + 1)
0nq ×m
Aq
0nq ×nr 0nq ×n
xr (t + 1) = 0nr ×nq
Ar
0nr ×n  xr (t) + 0nr ×m  u(t)
A
x(t)
x(t + 1)
Cq
0n×nr
B


xq (t)
y(t) = 0ℓ×nq Cr C xr (t) + Du(t)
x(t)
(18)
Note-se que como uq (t) e ur (t) são impulsos de Dirac que ocorrem em instantes à priori
desconhecidos, são sempre nulos exceto nesse instante. Por isso podem podem ser removidos
do modelo alargado sem que isso impeça a convergência do estado alargado para o seu valor
verdadeiro. Por isso, na prática, só é necessário selecionar os parâmetros Aq , Cq , Ar e Cr
dos modelos internos das perturbações.
12
Filtro de Kalman
3.2.1
Modelo Interno dum degrau
Como o degrau é o integral do impulso o seu modelo interno é um integrador, ou seja
xq (t + 1) = xq (t)
Aq = 1
⇒
,
(19)
q(t) = xq (t)
Cq = 1
sendo o de r/t idêntico.
3.3
Modelo Interno da rampa
Uma rampa pode ser obtida através da dupla integração do impulso de Dirac pelo que o seu
modelo interno

 xq1 (t + 1)
xq2 (t + 1)

q(t)
3.3.1
é o do duplo integrador:

1
0

= xq1 (t)
 Aq =
1 1
= xq1 (t) + xq2 (t) ⇒


= xq2 (t)
Cq = 0 1
(20)
Modelo interno duma sinusoide com frequência ω0
Uma sinusoide com frequência ω0 pode ser gerado por um sistema de segunda ordem com
valores próprios λ1,2

x (t + 1) =

 q1
xq2 (t + 1) =


q(t) =
= e±jω0 = cos ω0 ± j sin ω0 a que corresponde o seguinte modelo interno:
cos(ω0 )xq1 (t) + sin(ω0 )xq2 (t)
− sin(ω0 )xq1 (t) + cos(ω0 )xq2 (t) ⇒
xq1 (t) + xq2 (t)
(21)

cos
ω
sin
ω

0
0
 Aq =
− sin ω0 cos ω0


Cq = 1 1
3.4
Estimação duma perturbação em degrau
Exemplo 3 Consideremos que existe uma pertubação q(t) no binátio do motor. O modelo
do motor com essa perturbação passa a ser
x1 (t + 1) = x1 (t) + x2 (t)
x2 (t + 1) = 0.9x2 (t) + 10u(t) + q(t)
y(t) = x1 (t)
13
Filtro de Kalman
Seja q(t) for um degrau de amplitude 10 no instante t=200 representado na Figura 10. A
q(t)
10
t
200
Figura 10: Perturbação em degrau.
Figura 11 mostra a evolução da posição e da velocidade do motor nestas condições onde se
pode ver que no instante da perturbação há uma variação do valor médio da velocidade que
provoca uma rampa na posição. Utilizámos o observador 1 para estimar a posição e elocidade
4
9
x 10
x1 (t)
8
7
6
5
4
3
2
t
1
0
0
100
200
300
400
500
600
700
800
900
100
200
300
400
500
600
700
800
900
1000
200
x2 (t)
150
100
50
0
t
−50
0
1000
Figura 11: Posição e velocidade do motor quando o binário sofre uma perturbação em degrau
no instante t=200.
do motor. A figura 12 comparaas estimativas com os valores verdadeiros onde parece que,
em regime permanente existe um erro constante nas estimativas. A existência deste erro
constante e confirmada na figura 13 que mostra a evolução dos erros das estimativas da
posição e da velocidade.
Ao incluir o modelo interno do degrau obtivemos o seguinte modelo alargado do sistema


  


xq (t + 1)
1 0 0
xq (t)
0
x1 (t + 1) = 0 1 1  x1 (t) +  0  u(t)
(22)
x2 (t + 1)
x2 (t)
10
1 0 0.9


xq (t)
(23)
y(t) = 0 0 1 x1 (t)
x2 (t)
14
Filtro de Kalman
4
5
x 10
4
Estado x1
Estimativa de x1
3
2
1
t
0
100
150
200
250
300
350
400
450
500
550
200
estado x2
Estimativa de x2
150
100
50
0
t
−50
0
100
200
300
400
500
600
700
800
900
1000
Figura 12: Comparação entre as estimativas da posição e da velocidade do motor com os
seus valor verdadeiros.
300
x̃1 (t) = x1 (t) − x̂1 (t)
250
200
150
100
50
0
−50
t
0
100
200
300
400
500
600
700
800
900
200
300
400
500
600
700
800
900
1000
80
x̃2 (t) = x2 (t) − x̂2 (t)
60
40
20
0
t
−20
0
100
1000
Figura 13: Evolução dos erros das estimativas da posição eda velocidade do motor.
em que xq (t) = q(t).
3.5
Observação de estados em sistemas com perturbações estocásticas
Nestas condições, podemos minimizar a covariância do erro das estimativas. No caso de q(t)
e r(t) serem sinais gaussianos isto é equivalente utilizar médias condicionadas de x(t) como
estimador de estado. Estas médias são condicionadas ao conhecimento de todas as entradas
e saı́das do sistema até um determinado instante t1 . O observador é classificado de acordo
com a relação de ordem entre t1 e t. Deste modo, o observador será:
• Um previsor se t < t1 .
Filtro de Kalman
15
• Um filtro se t1 = t.
• Um suavizador (smoother ) se t1 > t.
Os observadores que minimizam a covariância do erro são conhecidos como previsores, filtros
ou suavizadores de Kalman, dependendo do valor de t1 .
3.6
Estimação de estado em modelos de estado determinı́sticoestocásticos
Se no sistema (12), q(t) e r(t) forem ruı́do branco, podemos decompor x(t) da seguinte forma
x(t) = xd (t) + xs (t)
(24)
em que xd (t), a componente determinı́stica do estado, depende de u(t), e é o estado do
subsistema determinı́stico
xd (t + 1) = Axd (t) + Bu(t)
yd (t + 1) = Cxd (t) + Du(t),
(25)
e xs (t), a componente estocástica, é o estado do subsistema estocástico
xs (t + 1) = Axs (t) + q(t)
ys (t + 1) = Cxs (t) + r(t).
(26)
A equação (13) é o modelo dum sistema determinı́stico-estocástico. Consideremos a agora o
observador (8) para estimar x(t) e reescrevamos as suas equações na forma
x̂(t + 1) = (A − LC)x̂(t) + Bu(t) + q(t) + Lr(t)
ŷ(t) = C x̂(t) + Du(t).
(27)
Como este observador também é um sistema determinı́stico-estocástico, x̂(t) também pode
ser decomposto em componentes determinı́sticas e estocásticas, i.e.,
x̂(t) = x̂d (t) + x̂s (t)
(28)
em que x̂d é o obervador determinı́stico
x̂d (t + 1) = (A − LC)x̂d (t) + Bu(t) + Lyd
ŷd (t) = C x̂d (t) + Du(t)
(29)
Filtro de Kalman
16
e x̂s (t) o estado do observador estocástico
x̂s (t + 1) = (A − LC)x̂s (t) + q(t) + Lr(t)
ŷs (t) = C x̂s (t) + r(t).
(30)
Subtraindo a equação de estado do observador (29) à do subsistema determinı́stico (25)
obtemos uma equação que nos descreve a dinâmica da componente determinı́stica do erro
do observador:
x̃d (t + 1) = (A − LC) x̃d (t).
(31)
Como esta é equação dum sistema autónomo a componente determinı́stica do erro anula-se
sempre desde que seja assegurada a estabilidade do observador. Podemos então concluir
que, para minimizar a covariância do erro, só necessitamos de considerar o observador do
subsistema estocástico.
No que se segue iremos deduzir as equações do previsor (a um passso) e do filtro de Kalman.
Para esse efeito consideraremos apenas o subsistema estocástico já que, como acabámos de
ver, a componente determinı́stica da estimativa de estado acaba por se anular ao fim de
algum tempo.
3.7
Previsor de Kalman
T
Consideremos o sistema estocástico (26) com q T (t) r T (t) ruı́do branco de média nula,
Q S
q(t) T
T
q (t) r (t) = T
E
S R
r(t)
(32)
x̂s (t + 1|t) = Ax̂s (t|t − 1) + K(t) [ys (t) − C x̂s (t|t − 1)]
(33)
e tomemos o observador com ganho variante no tempo K(t)
onde
x̂s (t|t − 1) = E {xs (t)|y(t − 1), y(t − 2), . . . , y(1), xs(0)} ,
(34)
ou seja, x̂s (t|t − 1) é o valor esperado de xs (t) dados (conhecidos) os valores de y(t − 1), y(t −
2), . . . , y(1), xs (0). Para simplificarmos a notação designaremos, daqui para a frente,
17
Filtro de Kalman
x̂s (t|t − 1) como x̂s (t) e o erro x̃s (t|t − 1) = x(t)s − x̂s (t|t − 1) como x̃s (t). Este erro pode
ser calculado substraindo (33) à primeira equação de (26):
x̃s (t + 1) = Ax̃s (t) + q(t) − K(t)ỹs (t),
(35)
onde
ỹs (t) = ys (t) − C x̂s (t) = Cxs (t) + r(t) − C x̂s (t) = C x̃s (t) + r(t).
(36)
Problema: Determinar o ganho K(t) que minimize a covariância do erro P̃ (t) = E x̃s (t)x̃Ts (t) .
A primeira dificuldade é o facto de P̃ (t) ser uma matriz o que imediatamente sugere a seguinte pergunta:
Em que é que consiste a minimização duma matriz?
RESPOSTA: Na minimização duma norma pré-definida.
As normas mais comuns de matrizes reais são
• as normas induzidas:
n
o
kAkp = max kAxkp : kxkp = 1
• A norma max
kAkmax = max
aij
• Norma de Frobenius:
v
uX
n
u n X
kAkF = t
a2ij = traço(AT A)
i=1 j=1
Como P̃ (t) é uma matriz simétrica vamos socorrermo-nos da seguinte relação de ordem entre
matrizes simétricas para resolver o problema da sua minimização:
Sejam M1 e M2 duas matrizes simétricas com a mesma dimensão. Dizemos que a matriz
M1 é maior que M2 e designamos esse facto por M1 > M2 ou M1 ≻ M2 se M1 − M2 for
uma matriz semidefinida positiva . Se M1 − M2 for uma matriz semidefinida positiva então
dizemos que M1 é maior ou igual a M2 e designamos esse facto por M1 ≥ M2 ou M1 M2
18
Filtro de Kalman
(ver o apêndice A para a definição de matriz (semi)definida positiva e negativa e para a
enumeração de algumas das suas propriedades). Diremos então que P̃ ∗ (t) é mı́nima se e só
se
P̃ (t) P̃ ∗ (t), ∀matrizes de covariância P̃ (t) do erro x̃(t)
Antes de calcular P̃ (t) vamos definir as seguintes variáveis auxiliares:
Λ0 (t) = E ỹs (t)ỹsT (t) ∈ IRℓ×ℓ
G(t) = E [Ax̃s (t) + q(t)] ỹsT (t) ∈ IRn×ℓ
(37)
Calculemos agora o produto x̃s (t + 1)x̃Ts (t + 1) utilizando a equação (35):
x̃s (t + 1)x̃Ts (t + 1) = [Ax̃s (t) + q(t) − K(t)ỹs (t)] [Ax̃s (t) + q(t) − K(t)ỹs (t)]T =
= [Ax̃s (t) + q(t)] [Ax̃s (t) + q(t)]T − [Ax̃s (t) + q(t)] ỹsT (t)K T (t)
−K(t)ỹs (t) [Ax̃s (t) + q(t)]T + K(t)ỹs (t)ỹsT (t)K(t)T ,
e retiremos o seu valor esperado2 :
P̃ (t + 1) = E x̃s (t + 1)x̃Ts (t + 1) =
n
o
T
= E [Ax̃s (t) + q(t)] [Ax̃s (t) + q(t)] − E [Ax̃s (t) + q(t)]ỹsT (t) K T (t)
|
{z
}
G(t)
−K(t) E ỹs (t)[Ax̃s (t) + q(t)]T +K(t) E ỹs (t)ỹsT (t) K T (t)
|
{z
}
|
{z
}
Λ0 (t)
GT (t)
= A E [x̃s (t)x̃s (t)] AT + A E x̃s (t)q T (t) + E q(t)x̃Ts (t) AT + E q(t)q T (t)
|
{z
}
|
{z
} |
{z
}
|
{z
}
0n×n
P̃ (t)
T
0n×n
T
Q
T
−G(t)K (t) − K(t)G (t) + K(t)Λ0 (t)K (t) =
= AP̃ (t)AT + Q − G(t)K T (t) + K(t)GT (t) + K(t)Λ0 (t)K T (t).
(38)
Definemos a variável auxiliar
ψ(K, t) = −E x̃(t + 1)ỹ T (t) = K(t)Λ0 (t) − G(t) ∈ IRn×ℓ .
(39)
2
Notemos que E x̃(t)q T (t) = E q(t)x̃T (t) = 0n×n porque q(t) é ruı́do branco. De (35) e (36) temos,
para τ = 1, 2, ..., t
x̃s (1) =
x̃s (2) =
..
.
x̃s (t) =
[A − K(0)] x̃s (0) + q(0) + K(t)r(0)
[A − K(1)] [A − K(0)] x̃s (0) + [A − K(1)] q(0) + [A − K(1)] K(0)r(0)
0
Y
τ =t−1
!
[A − K(τ )] x̃s (0) +
t−1
X
τ =0
1
Y
υ=τ
!
[A − K(υ)] [q(τ ) + K(τ )r(τ )]
T
q (t) rT (t) é ruı́do branco então E q(t)q T (τ ) = 0n×n e E {q(t)r(τ )} = 0n×ℓ e, consequentemente
Como
E x̃(t)q T (t) = E q(t)x̃T (t) = 0n×n .
19
Filtro de Kalman
e calculemos
T
ψ(K, t)Λ0−1 (t)ψ T (K, t) =[K(t)Λ0 (t) − G(t)] Λ−1
0 (t) [K(t)Λ0 (t) − G(t)]
T
= K(t) − G(t)Λ−1
0 (t) [K(t)Λ0 (t) − G(t)]
T
=K(t)Λ0 (t)K T (t) − K(t)GT (t) − G(t)K T (t) + G(t)Λ−1
0 (t)G (t)
Podemos retirar desta equação o valor de K(t)GT (t) − G(t)K T (t) em função de ψ(K, t),
Λ0 (t), K(t) e G(t):
T
T
−1
T
G(t)K T (t) + K(t)GT (t) = −ψ(K, t)Λ−1
0 (t)ψ (K, t) + K(t)Λ0 (t)K (t) + G(t)Λ0 (t)G (t).
Subsituindo em (38) K(t)GT (t) − G(t)K T (t) por este valor:
P̃ (t + 1) = AP̃ (t)AT + Q − G(t)K T (t) + K(t)GT (t) + K(t)Λ0 (t)K T (t)
T
T
= AP̃ (t)AT + Q + ψ(K, t)Λ−1
0 (t)ψ (K, t) − K(t)Λ0 (t)K (t)
T
T
−G(t)Λ−1
0 (t)G (t) + K(t)Λ0 (t)K (t) .
Temos, assim,
T
−1
T
P̃ (t + 1) = AP̃ (t)AT + Q + ψ(K, t)Λ−1
0 (t)ψ (K, t) − G(t)Λ0 (t)G (t).
T
Como ψ(K, t)Λ−1
0 (t)ψ (K, t) 0n×n então
−1
T
T
P̃ (t + 1) = AP̃ (t)AT + Q − G(t)Λ−1
0 (t)G (t) + ψ(K, t)Λ0 (t)ψ (K, t)
T
AP̃ (t)AT + Q − G(t)Λ−1
0 (t)G (t).
Logo
h
i
T
min P̃ (t + 1) = AP̃ (t)AT + Q − G(t)Λ−1
0 (t)G (t)
K
e
ψ(K, t) = 0n×ℓ ⇒ K(t)Λ0 (t) − G(t) = 0n×ℓ ⇔
K(t) = G(t)Λ−1
0 (t).
(40)
Desta equação e da definição de ψ(K, t) em (39) podemos concluir que que a covariância do
erro P̃ (t + 1) é mı́nima quando
E x̃(t + 1)ỹ T (t) = 0,
(41)
ou seja, quando o erro da previsão do estado x(t + 1) for não correlacionado com o erro da
previsão da saı́da y(t). Pode-se provar que esta condição implica que o erro da previsão da
20
Filtro de Kalman
saı́da, ỹ(t) é ruı́do branco de média nula.
De (37) e (36) e do facto de r(t) ser ruı́do branco,
o
n
Λ0 (t) = E [C x̃s (t) + r(t)] [C x̃s (t) + r(t)]T =
= E C x̃s (t)x̃Ts (t)C T + C x̃s (t)r T (t) + r(t)x̃Ts (t)C T + r(t)r T (t)
= C E x̃s (t)x̃Ts (t) C T + C E x̃s (t)r T (t) + E r(t)x̃Ts (t) C T + E r(t)r T (t) =
|
|
{z
}
{z
} |
{z
}
|
{z
}
P̃ (t)
T
0n×ℓ
0ℓ×n
R
= C P̃ (t)C + R.
(42)
T
Por outro lado, também de (37) e (36) e de q(t)T r(t)T ser ruı́do branco,
o
n
G(t) = E [Ax̃s (t) + q(t)] [C x̃s (t) + r(t)]T =
= E Ax̃s (t)x̃Ts (t)C T + Ax̃s (t)r T (t) + q(t)x̃Ts (t)C T + q(t)r T (t)
= A E x̃s (t)x̃Ts (t) C T + A E x̃s (t)r T (t) + E q(t)x̃Ts (t) C T + E q(t)r T (t) =
|
{z
}
|
{z
} |
{z
}
|
{z
}
P̃ (t)
T
0n×ℓ
0n×n
S
= AP̃ (t)C + S.
(43)
Substituindo Λ0 (t) e G(t) pelos valores das equações (42) e (43), respectivamente, obtemos
o valor do ganho do previsor de Kalman:
K(t) =
G(t)Λ−1
0 (t)
h
ih
i−1
T
T
= AP̃ (t)C + S C P̃ (t)C + R
.
(44)
Para calcular K(t) temos que determinar P̃ (t). Sabemos que K(t) minimiza P̃ (t + 1) e que,
consequentemente:
h
i
T
P̃ (t + 1) = minK P̃ (t + 1) = AP̃ (t)AT + Q − G(t)Λ−1
0 G (t) =
h
ih
i−1 h
i (45)
T
T
T
T
= AP̃ (t)A + Q − AP̃ (t)C + S C P̃ (t)C + R
AP̃ (t)C + S .
Estas equações também são válidas para o previsor de Kalman de sistemas determinı́stico-
estocásticos pois, vimos atrás que erro da estimativa da componente determinı́stica do estado é assintoticamente nulo. Podemos resumir o previsor de Kalman para sistemas determinı́stico-estocásticos no seguinte algoritmo:
Filtro de Kalman
ALGORITMO: - Previsor de Kalman
1 - Inicialização:
1.1 - x̂(0) = µx = E {x(t)}
n
o
T
1.2 - P̃ (0) = Π(0) = E [x(0) − µ] [x(0 − µ)]
2 - Para t = 0, 1, . . .
2.1 - Calcular o ganho do previsor de Kalman:
h
ih
i−1
K(t) = AP̃ (t)C T + S C P̃ (t)C T + R
2.2 - Actualizar x̂(t + 1) e P̃ (t + 1):
x̂(t + 1) = Ax̂(t) + Bu(t) + K(t) [y(t) − C x̂(t) − Du(t)]
h
iT
P̃ (t + 1) = AP̃ (t)AT + Q − K(t) AP̃ (t)C T + S
2.3 - Calcular a previsão da saı́da ŷ(t):
ŷ(t) = C x̂(t) + Du(t)
Este algoritmo tem como entradas:
• y(t) - Saı́da do sistema.
• u(t) - Entrada do sistema.
• x̂(0) - Estimativa do estado inicial do sistema.
• Π(0) - Covariância do erro da estimativa do estado inicial.
• Q = E q(t)q T (t) .
• S = E q(t)r T (t) .
• R = E r(t)r T (t) .
• (A, B, C, D) - Parâmetros dos sistema.
E tem como saı́das:
• x̂(t) - Previsão do estado x(t) no instante t − 1.
21
22
Filtro de Kalman
• ŷ(t) - Previsão da saı́da y(t) no instante t − 1.
• K(t) - Ganho do previsor de Kalman.
• P̃ (t) - Covariância do erro do previsor de Kalman
Sendo um processo recursivo, o previsor de Kalman tem que ser inicializado com uma estimativa inicial do estado e uma covariância inicial para o erro dessa estimativa. Na maior parte
das vezes o conhecimento sobre o valor inicial do estado é nulo. Nessas situações qualquer
valor serve só que, como seguramente o valor escolhido é muito incerto, essa incerteza deve
ser refletida na covariância inicial do erro através dum valor elevado. Assim, quando não há
qualquer conhecimento sobre estado inicilal uma boa inicialização será:
• x̂(0) = 0n .
• P̃ = σ 2 In , σ grande.
A aqui In representa a matriz identidade de dimensão n.
Exemplo 4:
O sistema (6) foi perturbado por um ruı́do de estado branco gaussiano q(t) com covariância
Q:
q(t) =
0
q2 (t)
⇒ Q = E q(t)q T (t) =
0
0
0 E {q22 }
=
0 0
0 4
e as medições da posição foram perturbadas também por ruı́do branco gaussiano r(t) com
variância R = 2500. Foi projetado para este sistema um previsor de Kalman. A figuras
14-17 comparam as estimativas do previsor de Kalman com o valor verdadeiro do estado.
Na figura 14 os valores verdadeiros do estado não se distinguem dos valores iniciais a não
ser durante os instantes iniciais. Na ampliação da janela temporal 250 ≤ t ≤ 250 exibida na
figura 15 nota-se uma pequena diferença entre os valores verdadeiro e estimado da posição
(x1 (t)) embora não se distinga a velocidade da sua estimativa (x2 (t)).
As figuras 16 e 17 confirmam que os erros são pequenos. Também mostram o seu carater aleatório e evidenciam que, em regime permanente, são estacionários ou seja, após os
instantes iniciais parece que mantêm o seu valor médio e a sua variância.
As figuras 18-21 permitem comparar o desempenho do revisor de Kalman com os observadores
1, 2,3 e 4 anteriormente projetados e cujos ganhos são exibidos na tabela 1. Todas mostram
23
Filtro de Kalman
Valor verdadeiro
Previsor de Kalman
x1 (t)
4000
3000
2000
1000
0
−1000
0
200
400
200
400
Tempo
600
800
1000
600
800
1000
x2 (t)
100
50
0
−50
−100
0
Tempo
Figura 14: Variáveis de estado (a azul) e estimativas do previsor de Kalman (a preto e
praticamente indistinguı́vel) no sistema perturbado.
que o desempenho do previsor de Kalman e do observador 1 são claramente superiores. Para
uma melhor comparação entre estes estimadores calculámos os valores eficazes dos seus erros
definidos por
x̃irms
v
u
1000
u 1 X
=t
x̃2i (t),
1000 t=1
i = 1, 2.
Estes valores são apresentados na tabela 2. Podemos aı́ verificar que o previsor de Kalman
é o estimador de estado que apresenta o menor erro.
Tabela 2: Valores eficazes dos erros das estimativas do Previsor de Kalman e dos quatro
observadores de Luenberger.
x̃1rms
x̃2rms
Prev. Kalman Observador 1
32.40
36.36
8.59
9.08
Observador 2 Observador 3
42.53
62.99
10.36
19.89
Observador 4
110.55
61.45
24
Filtro de Kalman
Valor verdadeiro
Previsor de Kalman
x1 (t)
1000
900
800
700
600
200
210
220
230
240
250
230
240
250
Tempo
x2 (t)
40
20
0
−20
−40
200
210
220
Tempo
Figura 15: Ampliação numa janela temporal da evolução das variáveis de estado (a azul) e
das estimativas do previsor de Kalman (a preto) no sistema perturbado.
x̃1 (t)
200
0
−200
−400
−600
0
200
400
200
400
x̃2 (t)
Tempo
600
800
1000
600
800
1000
150
100
50
0
−50
0
Tempo
Figura 16: Erros do previsor de Kalman no sistema perturbado.
3.8
Previsor de Kalman estacionário e modelo de inovação
O ganho K(t) do previsor de Kalman, dado pela equação (44), depende da covariância do
erro P̃ (t), de R e S que são parâmetros do ruı́do e de A e C que são parâmetros do sistema.
Podemos também ver na equação (45) que a covariância do erro também só depende de
parâmetros do ruı́do Q, R e S e dos parâmetros do sistema A e C. Isto significa que o cálculo
de K(t) pode ser efetuado previamente sem necessidade observar os sinais do sistema. Por
1
outro lado, pode-se provar que se (A, Q 2 ) for acessı́vel, (A, C) observável e se a equação (45)
for inicializada com uma matriz P (0) ≻ 0n×n , então P (t) converge para a solução da equação
25
Filtro de Kalman
x̃1 (t)
60
40
20
0
−20
200
210
220
210
220
x̃2 (t)
Tempo
230
240
250
230
240
250
5
0
−5
−10
200
Tempo
Figura 17: Erros do previsor de Kalman no sistema perturbado (ampliação duma janela
temporal).
Valor verdadeiro
Observador 1
Observador 2
Observador 3
Observador 4
Previsor de Kalman
x1 (t)
4000
3000
2000
1000
0
−1000
0
200
400
200
400
x2 (t)
Tempo
600
800
1000
600
800
1000
200
0
−200
−400
−600
0
Tempo
Figura 18: Variáveis de estado (azul) e estimativas do previsor de Kalman (a preto) e dos
observadores 1, 2, 3, e 4 no sistema perturbado.
de algébrica de Riccati
−1 T
P̃ = AP̃ AT + Q − AP̃ C + S C P̃ C T + R
AP̃ C T + S .
(46)
Se P̃ (t) converge, o mesmo acontece o ganho do previsor de Kalman que estabiliza em
−1
K = AP̃ C + S C P̃ C T + R
.
(47)
O previsor de Kalman passa então a ser o sistema linear e invariante no tempo
x̂(t + 1) = Ax̂(t) + Bu(t) + K [y(t) − C x̂(t) − Du(t)]
ŷ(t) = C x̂(t) + Du(t).
(48)
26
Filtro de Kalman
Valor verdadeiro
Observador 1
Observador 2
Observador 3
Observador 4
Previsor de Kalman
x1 (t)
1200
1000
800
600
200
210
220
210
220
x2 (t)
Time
230
240
250
230
240
250
200
100
0
−100
−200
200
Time
Figura 19: Ampliação numa janela temporal da evolução das variáveis de estado (a azul) e
das estimativas do previsor de Kalman (a preto) e dos observadores 1, 2, 3 e 4 no sistema
perturbado.
Observador 1
Observador 2
Observador 3
Observador 4
Previsor de Kalman
x̃1 (t)
1000
500
0
−500
0
200
400
Time
600
800
1000
200
400
Time
600
800
1000
x̃2 (t)
600
400
200
0
−200
−400
0
Figura 20: Erros do previsor de Kalman e dos observadores 1, 2, 3 e 4 no sistema perturbado.
Este observador também é conhecido como previsor estacionário de Kalman. Muitas vezes
utiliza-se o previsor estacionário logo a partir do instante inicial. Nestas situações é necessário
resolver a equação de Riccatti (46) para se obter o ganho do previsor. Existem muitos
métodos para resolver esta equação. Entre eles destacamos o método de Jacobi que apesar
de não ser computacionalmente eificiente, quase sempre converge para a solução pretendida.
27
Filtro de Kalman
Observador 1
Observador 2
Observador 3
Observador 4
Previsor de Kalman
x̃1 (t)
400
200
0
−200
−400
200
210
220
210
220
x̃2 (t)
Tempo
230
240
250
230
240
250
200
100
0
−100
−200
200
Tempo
Figura 21: Erros do previsor de Kalman e do filtro de Kalman no sistema perturbado (ampliação duma janela temporal).
Neste método , P̃ é atualizado iteração i + 1 através de
−1 T
P̃ (i+1) = AP̃ (i) AT + Q − AP̃ (i) C + S C P̃ (i) C T + R
AP̃ (i) C T + S .
(49)
Sendo esta equação igual à equação de actualização da covariância do erro do previsor não
estacionário, equação (45), converge exatamente nas mesmas condições, ou seja, quando
1
(A, Q 2 ), (A, C) observável e P (0) ≻ 0.
O sistema deterministico-estocástico (12) é muitas vezes representado pelo modelo
xp (t + 1) = Axp (t) + Bu(t) + Ke(t)
y(t) = Cxp (t) + Du(t) + e(t)
(50)
em que e(t) é ruı́do branco com variância R e K o ganho do previsor estacionário de Kalman. Este modelo é conhecido como modelo de inovação (inovation model ). O subsistema
deterministico é também descrito pelo modelo (25) enquanto que o subsistema estocástico é
descrito pelo modelo de inovação
xps (t + 1) = Axps (t) + Ke(t)
yps (t) = Cxps (t) + e(t)
(51)
que tem como modelo inverso
xps (t + 1) = (A − KC) xps (t) + Kyps (t)
e(t) = −Cxps (t) + yps (t)
(52)
28
Filtro de Kalman
Como K minimiza a covariância do erro x̃ps (t) = xs (t) − xps (t), este erro tem que ser
estacionário. Se fizermos e(t) = r(t) e se subtrairmos (52) a (26) constatamos que x̃ps (t) é o
vetor de estado do sistema
x̃ps (t + 1) = (A − KC)x̃ps (t) + q(t) − Ke(t)
ỹps (t) = C x̃ps (t) + e(t).
Como x̃ps (t) é estacionário, este sistema tem que ser estável e, consequentemente, os valores
próprios de A−KC têm que estar todos no interior do cı́rculo unitário. Se, adicionalmente, o
sistema (A, B, C, D) for estável e yps (t) for escalar (yps (t) ∈ IR), então yps (t) é um processo
estacionário com densidade espetral
Φps (ejω ) = HK (ejω )HK (e−jω )R
(53)
com
HK (ejω ) = HK (z)|z=ejω = C(zIn − A)−1 K + 1
z=ejω
=
det [zIn − (A − KC)]
. (54)
det(zIn − A)
z=ejω
Como det [zIn − (A − KC)] e det(zIn − A) são polinómios de ordem n em z, HK (z) é uma
função racional, ou seja,
HK (z) =
1 + c1 z −1 + · · · + cn z −n
.
1 + a1 z −1 + · · · + an z −n
Deste modo, yps (t) é um processo auto-regressivo de média móvel (ARMA) e o modelo de
inovação (47) é uma realização no espaço de estados do modelo ARMAX
A(q −1 )y(t) = B(q −1 )u(t) + C(q −1 )e(t)
onde
A(q −1 ) = 1 + a1 q −1 + · · · + an q −n
B(q −1 ) = 1 + b1 q −1 + · · · + bn q −n
C(q −1 ) = 1 + Cc1 q −1 + · · · + cn q −n
em que q −1 é o operador de atraso, i.e.,
q −1 x(t) = x(t − 1)
e
B(z −1 )
= C(zIn − A)−1 B + D
A(z −1 )
C(z −1 )
= C(zIn − A)−1 K + 1
A(z −1 )
29
Filtro de Kalman
Exemplo 5:
As Figura 22 mostra a evolução dos elementos de P̃ (t) no previsor de Kalman considerado
no exemplo 4. Constatamos aı́ que P̃ (t) convergiu muito rapidamente para
5000
P̃11 (t)
2500
4000
2000
3000
1500
2000
1000
1000
500
0
0
500
Tempo
1000
0
0
P̃21 (t)
2500
2000
2000
1500
1500
1000
1000
500
500
0
500
Tempo 1000
500
Tempo 1000
P̃22 (t)
2500
0
P̃12 (t)
500
Tempo 1000
0
0
Figura 22: Evolução dos elementos de P̃ (t) no previsor de Kalman do exemplo 4
565.6453 54.1720
P̃ =
54.1720 16.9717
T
a que corresponde um ganho K(t) = K = 0.2022 0.0159 . Os erros do previsor estacionário têm valores eficazes
x̃1rms = 33.19
x̃2rms = 9.04
que são ligeiramente superiores ao do previsor não estacionário mas inferiores aos do observadores 1, 2, 3 e 4, apresentados na tabela 2. Como
B(z −1 )
z −2
=
A(z −1 )
1 − 1.9z −1 + 0.9z −2
1 − 1.698z −1 + 0.7339z −2
C(z −1 )
=
C(zIn − A)−1 K + 1 =
A(z −1 )
1 − 1.9z −1 + 0.9z −2
C(zIn − A)−1 B + D =
(55)
(56)
então
B(q −1 ) = q −2
(57)
A(q −1 ) = 1 − 1.9q −1 + 0.9q −2
(58)
C(q −1 ) = 1 − 1.698q −1 + 0.7339q −2
(59)
30
Filtro de Kalman
pelo que
y(t) − 1.9y(t − 1) + 0.9y(t − 2) = u(t − 2) + e(t) − 1.698e(t − 1) + 0.7339e(t − 2)
é o modelo ARMAX do qual o modelo de inovação
33.19
0
1 1
e(t)
u(t) +
x(t) +
xp (t + 1) =
9.04
10
0 0.9
y(t) = 1 0
(60)
(61)
(62)
do previsor estacionário de Kalman é uma realização.
Como o sistema não é estável (tem um valor próprio unitário) a componente estocástica da
saı́da não é um processo estacionário. No entanto, dado que o previsor de Kalman é sempre
C(z −1 )
estável, a função de transferência do ruı́do, H(z) =
também é inversamente estável
A(z −1 )
(os valores próprios de A − KC que são os zeros de C(z −1 ) são λk1,2 = 0.84890 ± 0.11528j
com valor absoluto |λk1,2 | = 0.85669 < 1).
4
Filtro de Kalman
Depois de medir y(t) ficámos com uma quantidade de informação superior à que possuı́amos
no instante t − 1. Consequentemente, após esta observação podemos corrigir a estimativa do previsor de Kalman obtendo uma nova estimativa com um erro menor (em média).
Designaremos esta estimativa por x̂(t|t) e será ótima se minimizar a covariância do erro
x̃(t|t) = x(t) − x̂(t|t). A estimativa ótima é produzida pelo Filtro de Kalman. Como a
parte determinı́stica das estimativas do previsor de Kalman tem um erro nulo só temos que
nos preocupar em minimizar a covariância do erro as componentes estocásticas. O filtro de
Kalman atualiza estas componentes através da equação
x̂s (t|t) = x̂s (t) + Kf (t)ỹs (t)
(63)
onde
x̂s (t) − Estimativa de x(t) produzida pelo previsor de Kalman no instante t − 1
Kf (t) − Ganho do filtro de Kalman.
ỹs (t) = ys (t) − C x̂s (t).
O erro de x̂s (t|t) é
x̃s (t|t) = xs (t) − x̂s (t|t) = xs (t) − x̂s (t) −Kf (t)ỹs (t) = x̃s (t) − Kf (t)ỹs (t)
|
{z
}
x̃s (t)
(64)
31
Filtro de Kalman
Na determinação de Kf utilizaremos de novo Λ0 definido na equação (37) e
Gf (t) = E x̃s (t)ỹsT (t) .
(65)
Calculemos o produto x̃s (t|t)x̃Ts (t|t):
x̃s (t|t)x̃Ts (t|t) = [x̃s (t) − Kf (t)ỹs (t)] [x̃s (t) − Kf (t)ỹs (t)]T =
= x̃s (t)x̃Ts (t) − x̃s (t)ỹsT (t)KfT (t) − Kf (t)ỹs (t)x̃Ts (t) + Kf (t)ỹs (t)ỹsT (t)KfT (t),
e retiremos o seu valor esperado
P̃ (t|t) = E x̃s (t)x̃Ts (t) − E x̃s (t)ỹsT (t) KfT (t) − Kf (t) E ỹs (t)x̃Ts (t) +
|
{z
} |
{z
}
|
{z
}
Gf (t)
GT
P̃ (t)
f (t)
+Kf (t) E ỹs (t)ỹsT (t) KfT (t) =
|
{z
}
(66)
Λ0 (t)
Gf (t)KfT (t)
− Kf (t)GTf (t) + Kf (t)Λ0 (t)KfT (t) =
= P̃ (t) − Gf (t)KfT (t) + Kf (t)GTf + Kf (t)Λ0 (t)KfT (t).
= P̃ (t) −
Tal como na determinação do ganho do previsor, definamos
Ψf (Kf , t) = −E x̃(t|t)ỹ T (t) = Kf Λ0 (t) − Gf (t).
(67)
e calculemos
T
−1
T
Ψf (Kf , t)Λ−1
0 (t)Ψf (Kf , t) = [Kf (t)Λ0 (t) − Gf (t)] Λ0 (t) [Kf (t)Λ0 (t) − Gf (t)] =
T
= Kf (t) − Gf (t)Λ−1
0 (t) [Kf (t)Λ0 (t) − Gf (t)]
= Kf Λ0 (t)KfT (t)−Kf (t)GTf (t) − Gf (t)KfT (t)+
T
+Gf (t)Λ−1
0 (t)Gf (t)
de onde retiramos Gf (t)KfT (t) + Kf (t)GTf (t) em função de Ψf (Kf , t), Λ0 , Kf (t) e Gf (t):
T
T
Gf (t)KfT (t) + Kf (t)GTf (t) = −Ψf (Kf , t)Λ−1
0 (t)Ψf (Kf , t) + Kf (t)Λ0 (t)Kf (t)+
T
Gf (t)Λ−1
0 (t)Gf (t).
Subsituindo em (66) Kf (t)GTf (t) − Gf (t)KfT (t) por este valor:
P̃ (t|t) = P̃ (t) − Gf (t)KfT (t) + Kf (t)GTf + Kf (t)Λ0 (t)KfT (t) =
−1
T
T
T
= P̃ (t) + Φf (Kf , t)Λ−1
0 (t)Φf (Kf , t) − Kf Λ0 (t)Kf (t) − Gf (t)Λ0 (t)Gf (t)+
+Kf (t)Λ0 (t)KfT (t) .
Temos assim
T
−1
T
P̃ (t|t) = P̃ (t) + ψf (Kf , t)Λ−1
0 (t)ψf (Kf , t) − Gf (t)Λ0 (t)Gf (t)
32
Filtro de Kalman
T
Como ψf (Kf , t)Λ−1
0 (t)ψf (Kf , t) 0n×n então
−1
T
T
P̃ (t|t) = P̃ (t) − G(t)Λ−1
0 (t)Gf (t) + ψf (Kf , t)Λ0 (t)ψf (Kf , t)
T
P̃ (t) − Gf (t)Λ−1
0 (t)Gf (t).
Logo
h
i
T
min P̃ (t|t) = P̃ (t) − Gf (t)Λ−1
0 (t)Gf (t)
(68)
Kf
e
ψf (Kf , t) = 0n×ℓ ⇒ Kf (t)Λ0 (t) − Gf (t) = 0n×ℓ ⇔
Kf (t) = Gf (t)Λ−1
0 (t).
(69)
Desta equação e de (67) podemos concluir que no filtro de Kalman erro de estimação de x(t)
no instante t, x̃(t|t) não é correlacionado com ỹ(t), o erro da previsão de y(t) no instante,
t − 1.
Da definição de Gf (t) em (65) e de (36),
o
n
Gf (t) = E{x̃(t) ỹsT (t) } = E{x̃(t)[ ys (t) −C x̂s (t)]T } = E x̃(t) [Cxs (t) + r(t) − C x̂s (t)]T
| {z }
|{z}
Cxs (t)+r(t)
[ys (t)−C x̂T (t)]T
o
n
T
T
= E{x̃(t) C(xs (t) − x̂s (t)) + r(t) } = E x̃(t) [C x̃(t) + r(t)] =
|
{z
}
x̃(t)
= E x̃(t)x̃T (t) C T + E x̃(t)r T (t) = P̃ (t)C T
|
{z
}
{z
}
|
P̃ (t)
0n×ℓ
Utilizando esta equação e a definição de Λ0 na equação (37) em (69), obtemos o ganho do
filtro
h
i−1
T
T
C
P̃
(t)C
+
R
Kf (t) = Gf (t)Λ−1
(t)
=
P̃
(t)C
.
0
(70)
h
i
A covariância do erro do filtro de Kalman é o valor de minKf P̃ (t|t) da equação (68), ou
seja,
T
P̃ (t|t) = P̃ (t) − Gf (t)Λ−1
0 (t)Gf (t).
(71)
T
Notemos que, sendo na pior das hipóteses Gf (t)Λ−1
0 (t)Gf (t) é uma matriz semidefinida
positiva, P̃ (t|t) P̃ (t) confirmando-se assim que as estimativas de xs (t) que utilizam a informação até ao instante t têm menor erro do que as previsões no instante t − 1. Como a
componente determinı́stica do erro das estimativas de estado é nula, estas equações também
Filtro de Kalman
33
são válidas para os sistemas determinı́stico-estocásticos. O filtro e previsor de Kalman para
este tipo de sistemas podem ser implementados pelo seguinte algoritmo:
ALGORITMO: - Filtro e previsor de Kalman
1 - Inicialização:
1.1 - x̂(0) = µx = E {x(t)}
n
o
1.2 - P̃ (0) = Π(0) = E [x(0) − µ] [x(0 − µ)]T
2 - Para t = 0, 1, . . .
2.1 - Calcular os ganhos do previsor e do filtro de Kalman:
h
ih
i−1
K(t) = AP̃ (t)C T + S C P̃ (t)C T + R
h
i−1
T
T
Kf (t) = P̃ (t)C C P̃ (t)C + R .
2.2 - Actualizar x̂(t + 1) e P̃ (t + 1):
x̂(t + 1) = Ax̂(t) + Bu(t) + K(t) [y(t) − C x̂(t) − Du(t)]
x̂(t|t) = x̂(t) + Kf (t) [y(t) − C x̂(t) − Du(t)]
h
iT
T
T
P̃ (t + 1) = AP̃ (t)A + Q − K(t) AP̃ (t)C + S
P̃ (t|t) = P̃ (t) − Kf (t)C P̃ (t)
2.3 - Calcular a previsão ŷ(t) e a saı́da filtrada ŷ(t|t):
ŷ(t) = C x̂(t) + Du(t)
ŷ(t|t) = C x̂(t|t) + Du(t)
As entradas deste algoritmo são:
• y(t) - Saı́da do sistema.
• u(t) - Entrada do sistema.
• x̂(0) - Estimativa do estado inicial do sistema.
• Π(0) - Covariância do erro da estimativa do estado inicial.
• Q = E q(t)q T (t) .
Filtro de Kalman
34
• S = E q(t)r T (t) .
• R = E r(t)r T (t) .
• (A, B, C, D) - Parâmetros dos sistema.
Sendo as saı́das:
• x̂(t) - Previsão do estado x(t) no instante t − 1.
• x̂(t|t) - Estimativa do estado x(t) no instante t.
• ŷ(t) - Previsão da saı́da y(t) no instante t − 1.
• ŷ(t|t) - Estimativa da saı́da y(t) no instante t.
• K(t) - Ganho do previsor de Kalman.
• Kf (t) - Ganho do filtro de Kalman.
• P̃ (t) - Covariância do erro do previsor de Kalman
• P̃ (t|t) - Covariância do erro do filtro de Kalman.
Muitas vezes os ruı́dos de estado e de medição são independentes. Nestas situações, S =
E q(t)r T (t) = 0n e as equações do filtro e previsor de Kalman podem ser simplificadas
para
x̂(t + 1) = Ax̂(t|t) + Bu(k)
h
i−1
Kf (t) = P̃ (t)C T C P̃ (t)C T + R
x̂(t|t) = x̂(t) + Kf (t) [y(t) − C x̂(t) − Du(t)]
P̃ (t|t) = [In − Kf (t)C] P̃ (t)
P̃ (t + 1) = AP̃ (t|t)AT + Q.
(72)
Exemplo 6:
O estado do sistema (6) foi estimado por um filtro de Kalman utilzando os dados do exemplo
4. As Figuras 23-26 comparam as estimativas deste filtro as do previsor de Kalman. Nas
figuras 23,24 e 25 não se nota qualquer diferença significativa entre estas estimativas. No
entanto, na figura 24 o erro de x1 (t) parece ser um pouco menor no filtro continuando a
não haver diferenças significativas no erro de x2 (t). Esta constatação pode ser confirmada
na figura 26 que compara os erros do previsor e do filtro de Kalman numa ampliação dum
janela temporal e na Tabela 3 que compara os valores eficazes destes erros.
35
Filtro de Kalman
Valor verdadeiro
Previsor de Kalman
Filtro de Kalman
x1 (t)
4000
3000
2000
1000
0
−1000
0
200
400
200
400
Tempo 600
800
1000
600
800
1000
x2 (t)
100
50
0
−50
−100
0
Tempo
Figura 23: Variáveis de estado (azul) e estimativas do previsor de Kalman (a preto) e do
filtro de Kalman (a vermelho) no sistema perturbado.
Valor verdadeiro
Previsor de Kalman
Filtro de Kalman
x1 (t)
1000
900
800
700
600
200
210
220
210
220
Tempo
230
240
250
230
240
250
x2 (t)
40
20
0
−20
−40
200
Tempo
Figura 24: Ampliação numa janela temporal da evolução das variáveis de estado (a azul) e
das estimativas do previsor de Kalman (a preto) e filtro de Kalman no sistema perturbado.
36
Filtro de Kalman
Previsor de Kalman
Filtor de Kalman
x̃1 (t)
200
0
−200
−400
−600
0
200
400 Tempo 600
800
1000
200
400
800
1000
x̃2 (t)
150
100
50
0
−50
0
Tempo
600
Figura 25: Erros do previsor de Kalman e do filtro de Kalman no sistema perturbado.
Previsor de Kalman
Filtro de Kalman
x̃1 (t)
60
40
20
0
−20
200
210
220
210
220
Tempo
230
240
250
230
240
250
x̃2 (t)
10
5
0
−5
−10
200
Tempo
Figura 26: Erros do previsor de Kalman e do filtro de Kalman no sistema perturbado (ampliação duma janela temporal).
Tabela 3: Valores eficazes dos erros das estimativas do Previsor de Kalman e do filtro de
Kalman.
Prev. Kalman Filtro de Kalman
x̃1rms
32.40
25.40
x̃2rms
8.59
8.46
37
Filtro de Kalman
5
Filtro de Kalman de sistemas perturbados ruı́dos de
estado e de medição não correlacionados
Muitas vezes sabe-se, à priori, que os ruı́dos de estado e de medição são independentes e,
consequentemente, não correlacionados. Isto acontece quase sempre quando as variáveis de
estado são variáveis fı́sicas, isto é, quando têm um significado fı́sico bem definido. Nestas
condições
E q(t)sT (t) = S = 0n×ℓ ,
(73)
e a equação (44) passa a ser
h
i−1
K(t) = AP̃ (t)C T C P̃ (t)C T + R
.
(74)
De (70),
Kf = P̃ (t)C
T
h
T
C P̃ (t)C + R
i−1
,
(75)
pelo que
K(t) = AKf (t).
(76)
Utilizando esta relação na equação de estado do previsor de Kalman tem-se
x̂(t + 1) = Ax̂(t) + Bu(t) + K(t) [y(t) − C x̂(t) − Du(t)] =
(77)
Ax̂(t) + Bu(t) + AKf (t) [y(t) − C x̂(t) − Du(t)] =
A {x̂(t) + Kf (t) [y(t) − C x̂(t) − Du(t)]} +Bu(t) =
|
{z
}
=x̂(t|t)
Ax̂(t|t) + Bu(t).
Nestas condições,o algoritmo do filtro e previsor de Kalman passa a ser os seguinte:
ALGORITMO: - Filtro e previsor de Kalman
1 - Inicialização:
1.1 - x̂(0) = µx = E {x(t)}
n
o
T
1.2 - P̃ (0) = Π(0) = E [x(0) − µ] [x(0 − µ)]
(78)
Filtro de Kalman
38
2 - Para t = 0, 1, . . .
2.1 - Calcular o ganho do filtro de Kalman:
h
i−1
T
T
Kf (t) = P̃ (t)C C P̃ (t)C + R .
2.2 - Actualizar x̂(t + 1) e P̃ (t + 1):
x̂(t|t) = x̂(t) + Kf (t) [y(t) − C x̂(t) − Du(t)]
x̂(t + 1) = Ax̂(t|t) + Bu(t)
h
iT
P̃ (t + 1) = AP̃ (t)AT + Q − K(t) AP̃ (t)C T + S
P̃ (t|t) = P̃ (t) − Kf (t)C P̃ (t)
2.3 - Calcular a previsão ŷ(t) e a saı́da filtrada ŷ(t|t):
ŷ(t) = C x̂(t) + Du(t)
ŷ(t|t) = C x̂(t|t) + Du(t)
6
Sintonização do Previsor e do Filtro de Kalman
6.1
Sistemas estáveis
Os parâmetros do filtro e do previsor de Kalman dependem da covariância do ruı́do
Q R
q(t) q(t) r(t) = T
E
S R
r(t)
(79)
Infelizmente, esta covariância raramente é conhecida. Para se projetar previsor e/ou um filtro
de Kalman será pois necessário estimar esta matriz a partir dos dados. Ora isto implica
a solução dum problema de identificação e, como tal, será abordado posteriormente. No
entanto, se o sistema for estável, consegue-se dimensionar o previsor de Kalman estacionário
utilizando quantidades que podem ser estimadas a partir dos dados. Consideremos, então,
o sistema determinı́stico-estocástico estável
x(t + 1) = Ax(t) + Bu(t) + q(t)
(80)
y(t) = Cx(t) + Du(t) + r(t) = yd (t) + ys (t)
em que q T (t) r T (t) é ruı́do branco de média nula, com covariância dada pela equação
(79), yd a saı́da do subsistema determinı́tico
xd (t + 1) = Axd (t) + Bu(t)
yd (t) = Cxd (t) + Du(t).
(81)
39
Filtro de Kalman
e ys (t) a saı́da do subsistema estocástico
xs (t + 1) = Axs (t) + q(t)
ys (t) = Cxs (t) + r(t).
(82)
Se simularmos o subsistema determinı́stico com condição inicial x̂d (0), obtemos o sinal ŷd (t).
Subtraindo este sinal a y(t) teremos
ỹd (t) = CAt x(0) + ys (t),
(83)
onde ys (t) é a saı́da do subsistema estocástico. Como o sistema é estável, lim At = 0n×n
t→∞
e, consequentemente, lim tildeyd (t) = ys (t). Isto significa que, a partir dum determinado
t→∞
instante t = t0 , o erro de simulação do sistema passa a ser igual à saida do subsistema
estocástico. Como o sistema é estável, a saı́da do subsistema estocástico é um sinal ergódico
pelo que, se tivermos um número razoável de dados, conseguimos estimar a com alguma
exatidão a sequência de autocorrelação de ys (t),
λys (τ ) = E {ys (t + τ )ys (τ )} ∈ IRℓ
×ℓ
.
(84)
através de
λ̂ys (τ ) =
N
−τ
X
1
ȳ(t + τ )ȳ T (t).
N − t0 − τ + 1 t=t
(85)
0
Consideremos agora a covariância do erro do previsor de Kalman
P̃ = E x̃s (t)x̃Ts (t) .
Como xs (t) = x̂s (t) + x̃s (t) e se poe provar que E x̂s (t)x̃Ts (t) = 0n×n , então
n
o
P = E [x̂s (t) + x̃s (t)] [x̂s (t) + x̃s (t)]T = E x̂s (t)x̂Ts (t) + E {x̃s (t)x̃s (t)} = P̂ + P̃
⇒ P̃ = P − P̂
Substituindo P̃ (t) por este valor na equação (47) obtemos
−1
T
T
K = AP C + S − AP̂ C
CP C + R − C P̂ C
.
T
T
(86)
Fazendo a mesma substituição em (46) temos
P − P̂ = AP AT + Q − AP̂ AT
−1
− AP C T + S − AP̂ C T
CP C T + R − C P̂ C T
.
(87)
Filtro de Kalman
40
Calculando
o
n
P = E xs (t + 1)xTs (t + 1) = E [Axs (t) + q(t)] [Axs (t) + q(t)]T
T
T
T
T
= AE xs (t)xTs (t)
A
+
A
x
(t)q
(t)
+
q(t)x
(t)
A
+
q(t)q T (t)
s
s
T
T
T
T
= AP A + AE xs (t)q (t) + q(t)xs (t) A + Q.
Como q(t) é ruı́do branco,
e
T
E xs (t)q T (t) = E xs (t)q T (t)
= 0n×n
P = AP AT + Q.
(88)
Utilizando este resultado em (87),
−1
P − P̂ = P − AP̂ AT − AP C T + S − AP̂ C T CP C T + R − C P̂ C T
,
de onde chegamos à equação de Riccati
T
−1 AP C T + S − AP̂ C T
P̂ = AP̂ AT + AP C T + S − AP̂ C T CP C T + R − C P̂ C T
(89)
Da segunda equação de (82)
ys (t)ysT (s) = [Cxs (t) + r(t)] [Cxs (t) + r(t)]T
= Cxs (t)xTs (t)C T + Cxs (t)r T (t) + r(t)xTs (t)C T + r(t)r T (t).
Como r(t) é ruı́do branco, é independente de xs (t). Logo E Cxs (t)r T (t) = CE r(t)xs (t)T
0n×ℓ e
λys (0) = CE xs (t)xTs (t) C T + E r(t)r T (t) = CP C T + R.
Definindo Gs = E xs (t + 1)ysT (t) temos
o
n
Gs = E [Axs (t) + q(t)] [Cxs (t) + r(t)]T
= AE xs (t)xs (t)T C T + E q(t)q T (t) = AP C T + S
(90)
(91)
pois r(t) e q(t) são ambos ruı́do branco. Substituindo CP C T + R por λys (0) e AP C T + S
por Gs em (86) e (89), obtemos
i−1
h
K = Gs − AP̂ C T λys (0) − C P̂ C T
i−1 T
h
Gs − AP̂ C T .
P̂ = AP̂ AT + Gs − AP̂ C T λys (0) − C P̂ C T
(92)
(93)
=
41
Filtro de Kalman
Vemos, a partir destas equações que, para implementar o filtro estacionário de Kalman
necessitamos apenas de λys (0) = E ys (t)ysT (t) e de Gs = E xs (t + 1)ysT (t) . A covariância
λys (0) pode ser estimada através de (85) com τ = 0, i.e.,
λ̂ys (0) =
N
X
1
ȳ(t)ȳ T (t).
N − t0 + 1 t=t
(94)
0
Para estimar Gs , calculemos a expressão de λys (τ ) para τ 6= 0:
λys (τ ) = E ys (t + τ )ysT (t) = CE [xs (t + τ ) + r(t + τ )] ysT (t) .
Como r(t) é ruı́do branco, E r(t + τ )ysT (t) = 0ℓ×ℓ e, consequentemente,
λys (τ ) = CE xs (t + τ )ysT (t) .
Por outro lado, a solução, a solução da equação de estado de (82) para t + τ é
xs (t + τ ) = Aτ xs (t0 ) + Aτ −1 q(t) + Aτ −2 q(t + 1) + · · · + q(t + τ − 1)
= Aτ −1 (Axs (t) + q(t)) +Aτ −2 q(t + 1) + · · · + q(t + τ − 1)
|
{z
}
xs (t+1)
τ −1
= A
xs (t + 1) + Aτ −2 q(t + 1) + · · · + q(t + τ − 1),
pelo que,
τ −1
A xs (t + 1) + Aτ −2 q(t + 1) + · · · + q(t + τ − 1) ysT (t)
= CAτ −1 E xs (t + 1)ysT (t) +Aτ −2 E q(t + 1)ysT (t) + . . .
|
{z
}
Gs
+E q(t + τ − 1)ysT (t)
= CAτ −1 Gs + Aτ −2 E q(t + 1)ysT (t) + · · · + E q(t + τ − 1)ysT (t) .
λys (τ ) = CE
Como q(t) é ruı́do branco, então q(t+k), k > 0, é independente de ys (t) e, consequentemente,
λys (τ ) = CAτ −1 Gs .
Assim, se dispusermos de λ̂ys (τ ) para τ = 1, · · · , n1 > n, Teremos,
 


λ̂ys (1)
C
 λ̂ (2)   CA 
 

 ys
 ..  =  ..  Gs + Ξ
 .   . 
CAn1 −1
λ̂ys (n1 )
(95)
42
Filtro de Kalman
onde Ξ é o vetor dos erros das estimativas λ̂ys (τ ), τ = 1, . . . , n1 . Gs pode então ser determinado pelo estimador de mı́nimos quadrados
Ĝs = On† 1 Λnys1
(96)
onde

C
CA
..
.





On1 = 
 ∈ IRn1 ℓ×n ,


n1 −1
CA


λ̂ys (1)
 λ̂ (2) 

 ys
Λnys1 =  .  ∈ IRn1 ℓ×ℓ ,
 .. 
λ̂ys (n1 )
(97)
(98)
e On† 1 significa pseudoinverso de On1 que neste caso é
−1 T
On1 .
On† 1 = OnT1 On1
De (88), (90) e (91)
Q = P − AP AT
R = λ(0) − CP C T
S = Gs − AP C T .
Q S
tem que semidefinida ou definida positiva pois é uma
Por outro lado a matriz
ST R
matriz covariância. Consequentemente
As − AP AT Gs − AP C T
0n×n .
(99)
GTs − CP AT λys (0) − CP T
Pode-se provar que esta é uma condição necessária e suficiente para para a existência dum
modelo de inovação e que, consequentemente a equação de Riccati (89) tenha solução não
negativa. Nestas condições, esta equação pode ser resolvida pelo algoritmo de Jacobi inicializado com P̂ (0) = 0n×n . O único problema é que, quando λ(0) e Gs são estimados a partir
dos dados, não é garantido que a matriz (99) seja não negativa. Sabe-se, no entanto, que
uma condição necessária para que

λys (1)
λys (0)
 λT (1)
λys (0)
 ys
Ry (n1 ) =  ..
..
 .
.
T
T
λys (n1 ) λys (n1 − 1)
esta matriz seja não negativa é

···
λys (n1 )
· · · λys (n1 − 1)

 0(n1 +1)×(n1 +1) ∀n1 >0 .
..
..

.
.
···
λys (0)
(100)
Filtro de Kalman
43
Embora possa não ser suficiente, é

λ̂ys (1)
λ̂ys (0)
 λ̂T (1)
λ̂ys (0)
 y
R̂ys (n1 ) =  s.
..
 ..
.
T
T
λ̂ys (n1 ) λ̂ys (n1 − 1)
então necessário que asseguremos que

···
λ̂ys (n1 )
· · · λ̂ys (n1 − 1)

(101)
 εIn1 +1
..
..

.
.
···
λ̂ys (0)
em que ε é uma constante real positiva. Assim, se min ΛRys < ε, em que min ΛRys é o
menor valor próprio de Rys , devemos somar ε − min ΛRys In1 +1 a Rys , i.e,
Rys ← Rys + (ε − νRysmin )In1 +1
para assegurar a que o menor valor próprio de Rys passe a ser ε. Isto equivale a somar
ε − min ΛRys à variância λys (0), i.e.,
λys ← λys + ε − min ΛRys .
Só que esta correção pode não ser suficiente. Infelizmente só podemos verificar se é após
a resolução da equação (93). Se a solução desta equação não for definida ou semidefinida
positiva temos que voltar a somar a uma constante positiva Kλ a λys (0) e, de novo, voltar
a resolver (93). Este processo só deve terminar quando a solução desta equação for não
negativa. Conseguimos obter rapidamente uma solução deste tipo se a constante Kλ tiver
um valor elevado. No entanto, esta rapidez é conseguida à custa de um pior desempenho do
previsor, pois, quanto maior for o valor de Kλ , maior vai ser o desvio de R̂ys (n1 ).
O conhecimento de Gs e λys (0) permite-nos implementar o previsor de Kalman mas não é
suficiente para a implementação do filtro. De facto, o ganho do filtro é
i
h
Kf = P̃ C T C P̃ C T + R = P̃ C T λys (0) − C P̂ C T
(102)
e, por isso, necessita do conhecimento de P̃ C T que não pode ser obtido só através de Gs
e λys (0). No entanto, quando, como acontece frequentemente, o ruı́do de medição r(t) é
independente do ruı́do do processo q(t), S = 0n e, consequentemente,
Gs = AP C T .
Se A for uma matriz não singular, teremos então
P̃ C T = P C T − P̂ C T = A−1 Gs − P̂ C T
44
Filtro de Kalman
e
i
h
Kf = (A−1 Gs − P̂ C T ) λys (0) − C P̂ C T .
(103)
Exemplo 7 (Sintonização experimental dum Filtro de Kalman):
A velocidade do motor que temos vindo a considerar foi medida por um taquı́metro cujo
erro de medição é ruı́do branco gaussiano de média nula e variância desconhecida. Para
se obterem medições da velocidade com menor erro, utilizou-se um filtro de Kalman. Neste
exemplo, apresentaremos os resultados que obtivemos utilizando o método de sintonização
do filtro exposto nesta secção.
Antes de mais recordemos que o motor é descrito pela equação de estado
x1 (t + 1) = x1 (t) + x2 (t)
(104)
x2 (t + 1) = 0.9x2 (t) + 10u(t) + q1 (t)
em que q1 (t) é ruı́do branco gaussiano de média nula com variância E {q12 (t)}, x1 (t) é a
posição do veio e x2 (t) a sua velocidade angular. Ao medirmos a velocidade estamos a
considerar x2 (t) como saı́da do sistema sendo
y(t) = x2 (t) + r(t)
(105)
em que aqui r(t) é o erro de medição da velocidade. Estas equações formam um modelo de
estado (A, B, C, D, ) em que
1 1
0
A=
,
B=
,
0 0.9
10
C= 0 1 ,
D = 0.
A matriz da observabilidade deste sistema é
C
0 1
O=
=
.
CA
0 0.9
(106)
(107)
Como a primeira coluna de O é nula esta matriz tem caraterı́stica 1 e o sistema não é
observável. É fácil ver que x1 é o subespaço dos estados não observáveis e que, consequentemente, o sistema pode ser descrito pelo modelo de primeira ordem
x(t + 1) = 0.9x(t) + 10u(t) + q(t)
y(t) = x(t) + r(t),
(108)
em que x(t) = x2 (t) e q(t) = q1 (t).
As medições da velocidade foram perturbadas por ruı́do branco gaussiano de média nula e
45
Filtro de Kalman
Tabela 4: Sequência de autocorrelação de ys (t) para τ = 0, 1, . . . , 9 (λys (τ ) valor verdadeiro
e λ̂ys (τ ) valor estimado).
τ
λys (τ )
λ̂ys (τ )
Erro (%)
0
1
2
3
4
5
6
7
8
9
30.05 18.95 17.05 15.35 13.81 12.43 11.19 10.07 9.07 8.16
33.62 22.11 19.45 17.72 15.55 15.06 14.05 12.98 12.01 10.96
11.88 16.68 14.07 15.47 12.59 21.18 25.58 28.94 32.56 34.33
variância 9. Na tabela 4 podemos comparar os valores verdadeiros e estimados da sequência
de autocorrelação λys (τ ) para τ = 0, 1, . . . , 9 em que ys (t) é a componente estocástica da
saı́da, dada por
xs (t + 1) = 0.9x(t) + q(t)
ys (t) = x(t) + r(t).
(109)
Os erros elevados podem ser explicados pela baixa relação sinal ruı́do de ys (t) que é de cerca
de 2.57 dB, significando este valor que o ruı́do de medição representa cerca de 75% do sinal.
Observamos que estes erros aumentam quando o τ aumenta. Com os valores estimados desta
sequência determinamos Ĝs através do estimador (96) para n1 = 1, 2, . . . , 9. A tabela 5
Tabela 5: Valor verdadeiro e valores estimados de Gs para n1 = 1, 2, . . . , 9.
Gs
18.95
Erro(%)
n1 = 1
22.11
16.68
n1 = 2
21.89
15.51
Ĝs
n1 = 3 n1 = 4 n1 = 5
21.88
21.79
21.93
15.50
14.99 15.762
n1 = 6 n1 = 7 n1 = 8
22.11
22.27
22.42
16.66
17.52
18.33
n1 = 9
22.55
18.99
compara esses valores com o valor verdadeiro. Vemos que o erro começa por diminuir até
n1 = 4 começando a crescer a partir daı́. A diminuição inicial do erro pode ser explicado
pelo maior número de observações no estimador de mı́nimos quadrados. No entanto, o erro
das observações que se vão acrescentando é cada vez maior e, a partir de certa altura o seu
efeito anula o do aumento do número de observações.
As tabelas 6 e 7 comparam os ganhos dos previsores e dos filtros de Kalman calculados com
os valores exatos de Λys (0) e Gs com os que foram claculados com os valoes estimados.
Podemos observar que os erros relativos são inferiores aos de λ̂ys (0) e de Ĝs o que se deve a
um efeito de compensação entre estes erros.3 Os erros relativos dos ganhos do previsor e do
3
De (92) e (103) podemos ver que um valore em excesso de Gs faz aumentar os ganhos K e Kf sendo
isto compensado por um valor em excesso de λys (0) que faz diminuir estes ganhos.
46
Filtro de Kalman
Tabela 6: Ganhos dos previsores de Kalman calculados utilizando os valores verdadeiros de
λYS (0) e de Gs e valores estimados para n1 = 1, 2, . . . , 9.
K
0.40
Erro(%)
n1 = 1
0.426
6.06
n1 = 2
0.420
4.54
K̂
n1 = 3 n1 = 4 n1 = 5
0.420
0.417
0.421
4.53
3.87
4.86
n1 = 6 n1 = 7 n1 = 8
0.426
0.430
0.435
6.05
7.18
8.25
n1 = 9
0.438
9.16
Tabela 7: Ganhos dos filtros de Kalman calculados utilizando os valores verdadeiros de λ(0)
e de Gs e valores estimados para n1 = 1, 2, . . . , 9.
K
0.45
Erro(%)
n1 = 1
0.473
6.06
n1 = 2
0.467
4.54
K̂
n1 = 3 n1 = 4 n1 = 5
0.466
0.464
0.468
4.53
3.87
4.86
n1 = 6 n1 = 7 n1 = 8
0.473
0.478
0.483
6.05
7.18
8.25
n1 = 9
0.487
9.16
filtro são iguais e têm uma evolução semelhante à dos erros de Ĝs : Começam por decrescer
com n1 atingindo um mı́nimo para n1 = 4 começando a crescer a partir daı́. A tabela 8
Tabela 8: Valores eficazes dos erros das estimativas dos Previsores de Kalman (PK) e do
filtros de Kalman (FK) dimensionados a partir dos valores exatos de λys (0) e Gs (Teórico) e
dos valores estimados com n1 = 1, . . . , 9.
PK
FK
Teórico
2.6569
1.9886
n1 = 1
2.6617
1.9966
x̃rms
n1 = 2 n1 = 3 n1 = 4 n1 = 5 n1 = 6 n1 = 7
2.6601 2.6601 2.6595 2.6604 2.6617 2.6631
1.9939 1.9939 1.9929 1.9945 1.9966 1.9988
n1 = 8
2.6646
2.0012
n1 = 9
2.6659
2.0034
mostra os valores eficazes dos erros das estimativas dos previsores e dos filtros de Kalman
dimensionados a partir dos valores exatos de λys (0) e Gs (coluna “Teórico”) e dos valores
estimados com n1 = 1, . . . , 9 numa experiência diferente da que serviu para estimar os
parâmetros de projeto do filtro mas com o mesma entrada e o mesmo número de pontos (os
ruı́dos de processo e de observação é que são diferentes). Podemos constatar que praticamente
não há diferenças entre os erros do previsor e do filtro teóricos e dos dimensionados a partir
dos dados.
47
Filtro de Kalman
6.2
Sistemas instáveis
Num sistema instável a componente estocástica da saı́da não é estacionária e, consequentemente, deixa de ser possı́vel estimar a sua sequência de covariância através de (85). No
entanto, não é comum sistemas instáveis funcionarem em malha aberta pois, se assim fosse,
poderiam causar danos importantes nas instalações em que estão alojados. Para evitar que
isto aconteça são controlados por um sistema de controlo que tem como primeira função
estabilizá-los. Nestas condições, o seu funcionamento normal é estável e em malha fechada.
O problema de estimação de estado passa a ser o da observação de estado dum sistema
estável (o sistema em malha fechada). Como a componente estocástica saı́da é agora um
sinal estacionário e ergódico, podemos estimar a sua sequência de covariância a partir duma
única realização e determinar o ganho do previsor de Kalman a partir dessa sequência tal
como fizemos na secção anterior. O cálculo do ganho do filtro de Kalman pode não ser
tão simples pois, devido à realimentação que é necessária à estabilização do sistema, surge
sempre uma correlação entre o ruı́do de estado e o de medição. No entanto, essa correlação
pode ser parcialmente calculada, e há casos em que isso pode ser suficiente para calcular o
vetor P̃ C T , que é o que necessitamos para determinar o ganho do filtro.
No entanto, nalgumas situações o sistema instável pode funcionar em malha aberta sem
provocar estragos significativos. É o caso em que a estabilidade se deve à existência dum ou
mais valores próprios no ponto 1 + j0, ou seja, quando a instabilidade se deve ao fato do
sistema possuir pelo menos um integrador discreto. Um destes casos é o do passeio aleatório
que é o modelo utilizado em problemas de seguimento to (tracking) de objetos móveis.
6.2.1
Passeio aleatório
Consideremos então o caso mais simples onde x(t) ∈ IR é um passeio aleatório descrito por
x(t + 1) = x(t) + q(t)
(110)
onde q(t) é ruı́do branco de média nula com variância Q. Este processo é medido através
dum sensor perturbado por ruı́do branco r(t), independente de q(t) e com variância R
y(t) = x(t) + r(t)
(111)
48
Filtro de Kalman
Estas duas equações formam um modelo de estado com A = 1, B = 0, C = 0 e D = 0 e x(t)
pode ser estimado pelo filtro estacionário de Kalman
x̂(t|t) = x̂(t) + Kf [y(t) − x̂(t)]
x̂(t + 1) = x̂(t) + K [y(t) − x̂(t)]
(112)
onde
Kf = K =
P̃
P̃ + R
(113)
são, respetivamente, os ganho do filtro e do previsor de Kalman. P̃ é a variância de x̃(t) =
x(t) − x̂(t) e é a solução positiva da equação
p
P̃ 2
Q2 + 4QR
Q
P̃ = P̃ + Q −
.
⇒ P̃ = +
2
2
P̃ + R
(114)
Para podermos dimensionar o previsor e o filtro de Kalman temos que extrair Q e R a partir
de y(t). Notemos, no entanto, que
x(t) =
t−1
X
q(t)
t=ti
em que ti é o instante onde se inicia o passeio aleatório e que
E {x(t)x(t − τ )} = (t − |τ | − ti − 1)Q
sendo
E {y(t)y(t − τ )} =
(t − t0 − 1)Q + R, τ = 0
(t − |τ | − ti − 1)Q, τ =
6 0
Uma vez que E {y(t)y(t − τ )} varia ao longo do tempo (depende de t) não temos forma de
estimar esta sequência diretamente de y(t). No entanto, notemos que o diferencial
δ1y (t) = y(t) − y(t − 1) = x(t) + r(t) − x(t − 1) − r(t − 1)
= x(t) − x(t − 1) +r(t) − r(t − 1) = q(t − 1) + r(t) − r(t − 1)
{z
}
|
(115)
q(t−1)
é estacionário e ergódico. Deste modo, é possı́vel estimar

 Q + 2R, τ = 0,
−R,
|τ | = 1
λδ1y (τ ) =

0,
|τ | > 1
(116)
49
Filtro de Kalman
através de
N
−τ
X
1
λ̂δ1y (τ ) =
δ1y (t + τ )δ1y (t).
N − 1 − t0 − τ + 1 t=t
(117)
0
Resolvendo em ordem a Q̂ e R̂ o sistema de equações
Q̂ + 2R̂ = λ̂δ1y (0)
⇔
−R̂ = λ̂δ1y (1)
Q̂ = λ̂δ1y (0) + 2λ̂δ1y (1)
,
R̂ = −λ̂δ1y (1)
(118)
obtemos as estimativas de Q e R que necessitamos para calcular a covariância do erro do
previsor de Kalman e o ganho do filtro e do previsor a partir das equações (114) e (113)
No caso geral, se bem que não muito comum, em que há correlação entre q(t) e r(t), ou seja,
E {q(t)r(t)} = 0, terı́amos
P̃ + S
P̃ + R
P̃
=
P̃ + R
K =
Kf
(119)
(120)
onde P̃ é a solução positiva de
(P̃ + S)2
Q − 2S
+
P̃ = P̃ + Q −
⇒ P̃ =
2
P̃ + R
p
Q (Q + 4R − 4S)
2
(121)
e
Q + 2R − 2S = λδ1y (0)
−R + S = λδ1y (1).
(122)
Vemos, assim que não é possı́vel extrair Q, R e S só a partir de δy (t). Para resolver este
problema temos que econtrar uma nova equação que relacione estes parâmetros e que seja
independente de (122). Tomemos, então,
δ2y (t) = y(t) − y(t − 2) = x(t) + r(t) − x(t − 2) + r(t − 2)
= x(t − 1) + q(t − 1) + r(t) − x(t − 2) + r(t − 2)
= x(t − 1) − x(t − 2) +q(t − 1) + r(t) − r(t − 2)
|
{z
}
(123)
q(t−2)
= q(t − 2) + q(t − 1) + r(t) − r(t − 2).
A variância deste sinal é
2
λ2y (0) = E δ2y
(t) = 2Q + 2R − 2S.
(124)
50
Filtro de Kalman
Só que
δ2y (t) = y(t) − y(t − 2) = y(t) − y(t − 1) + y(t − 1) − y(t − 2) = δ1y (t) + δ1y (t − 1) (125)
{z
} |
{z
}
|
δ1y (t)
δ1y (t−1)
sendo
2
2
λ2y (0) = E [δ1y (t) + δ1y (t − 1)]2 = E δ1y
(t) + E δ1y
(t − 1) + 2E {δ1y (t)δ1y (t − 1)}
= λ1y (0) + λ1y (0) + λ1y (1) = 2λ1y (0) + λ1y (1).
(126)
Vemos, assim, que esta nova equação é linearmente dependente das equações do sistema
(122), pois pode ser obtida somando o dobro da primeira equação à segunda. É fácil ver
que a situação é idêntica para qualquer outro membro da sequência de covariância deste ou
doutro sinal diferencial definido a partir de y(t). Assim, só é possı́vel estimar as estatı́sticas
do ruı́do desde que uma delas seja conhecida à priori. Normalmente, os ruı́dos de processo
e de medição são independentes, pelo que S = 0, sendo Q e R estimados a partir de (118).
Q̂ Ŝ
Para garantir a convergência de (45) temos que assegurar que
02×2 . Se S = 0
Ŝ R̂
esta condição é equivalente a Q > 0 e R > 0.
Exemplo 8 (Passeio aleatório):
A Figura 27 mostra uma realização de
y(t) = x(t) + r(t)
(127)
onde r(t) é ruı́do branco gaussiano de média nula e variância unitária e x(t) é processo de
média móvel
x(t + 1) = x(t) + q(t)
(128)
com q(t) também ruı́do branco gaussiano de média nula com variância unitária independente
de r(t). A natureza não estacionária deste sinal é bem visı́vel nesta figura. A figura 28
apresenta o sinal δ1y (t) = y(t) − y(t − 1) onde podemos constatar que se trata de um sinal
estacionário. A Tabela 9 compara os valores estimados de λδ1y (0) e λδ1y (1) com os respetivos
valores verdadeiros. Podemos ver que o erro é relativamente elevado para λδ1y (0) e λδ2y (0),
sendo pequeno para λδ1y (1). O erros nas estimativas destas covariâncias conduzem uma
estimativa de Q com um erro moderado e a uma estimativa de R com um erro pequeno.
51
Filtro de Kalman
10
y(t)
8
6
4
2
0
−2
−4
0
20
40
Tempo
60
80
100
Figura 27: Realização de y(t) definido em (127) e (128)
4
δ1y (t)
3
2
1
0
−1
−2
−3
−4
−5
0
20
40
60
80
100
Tempo
Figura 28: Realização de δ1y (t)
Tabela 9: Valores verdadeiros e estimados de λδ1y (0) e λδ1y (1).
Valor verdadeiro
Valor estimado
Erro (%)
λδ1y (0)
3.00
2.74
8.65
λδ1y (1)
-1.00
-1.02
1.77
Tabela 10: Variâncias do ruı́do.
Valor verdadeiro
Valor estimado
Q̂
R̂
1.00 1.00
0.71 1.02
Na Tabela 11 observamos que, relativamente aos seus valores teóricos, os ganhos estimados
do previsor e filtro de Kalman apresentam desvios perfeitamente aceitáveis. Para testar o
52
Filtro de Kalman
Tabela 11: Ganhos do Previsor e Filtro de Kalman.
Valor teórico
Valor estimado (impondo S=0)
Erro (%)
K
Kf
0.6180 0.6180
0.5552 0.5552
10.17 10.17
previsor e o filtro de Kalman estimados gerámos uma nova realização de y(t). Podemos
4
x(t)
Valor verdadeiro
2
Previsor teórico
Previsor estimado
0
−2
−4
−6
−8
−10
−12
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 29: Comparação das estimativas dos previsores de Kalman teórico e estimado com o
valor verdadeiro de x(t).
ver, nas figuras 29 e 30 e na tabela 12, que os previsores de Kalman têm um desempenho
equivalente. Na mesma tabela e nas figuras 31 e 32 constatamos que o mesmo acontece com
os filtros de Kalman.
Tabela 12: Valores eficazes dos erros das estimativas dos Previsores de Kalman e do filtros
de Kalman teóricos e estimados.
PK
FK
x̃rms
Teórico Estimado
1.2891
1.2964
0.7687
0.7800
53
Filtro de Kalman
4
ep (t) = x(t) − x̂(t)
Erro do previsor teórico
Erro do previsor estimado
3
2
1
0
−1
−2
−3
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 30: Erros estimativas dos previsores de Kalman teórico e estimado.
4
x(t)
2
Valor verdadeiro
Filtro teórico
Filtro estimado
0
−2
−4
−6
−8
−10
−12
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 31: Comparação das estimativas dos filtros de Kalman teórico e estimado com o valor
verdadeiro de x(t).
6.2.2
Integrador com constante de tempo
Consideremos agora o sistema determinı́stico-estocástico
x(t + 1) = Ax(t) + Bu(t) + q(t)
y(t) = Cx(t) + Du(t) + r(t),
(129)
54
Filtro de Kalman
2.5
ef (t) = x(t) − x̂(t|t)
Erro do filtro teórico
Erro do filtro estimado
2
1.5
1
0.5
0
−0.5
−1
−1.5
−2
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 32: Erros estimativas dos filtros de Kalman teórico e estimado.
onde
b1
1 a21
,
, B =
A =
b2
0 a22
q1 (t)
q(t) =
C = c1 c2 , D = d 1 ,
q2 (t)
(130)
com |a22 | < 1 e
E




 q1 (t) q(t)
T


q (t − τ ) r(t − τ )
q2 (t) q1 (t − τ ) q2 (t − τ ) r(t − τ )
= E
r(t)


r(t)
 

Q
Q
S

1
1
1
1
2



 Q12 Q22 S2  , τ = 0
=
S1 S2 R





0,
τ=
6 0
(131)
Simulando o sistema com condições iniciais nulas e subtraindo o valor simulado à saı́da do
sistema, obtemos o sinal


a21 1 − at22
1
ȳ(t) = CAT x(0) + ys (t) = C 
1 − a22  x(0) + ys (t).
0
at22
(132)
55
Filtro de Kalman
Quando t → ∞,
lim ȳ(t) = ȳs (t) = c1
t→∞
c1 a21
c1 a21
x(0) + y(s) = c1 x1 (0) +
x2 (0) + ys (t).
1 − a22
1 − a22
(133)
em que x1 (t) e x2 (t) são as coordenadas de x(t). Vemos assim que, ao fim de algum tempo,
ȳ(t) difere de ys (t) a menos duma constante. Mas, devido ao valor próprio de A em 1, ys (t)
não é estacionário, e consequentemente não érgódico, pelo que não podemos estimar a sua
sequência de autocorrelação a partir duma realização. No entanto, se calcularmos
a2 xs (t−1)+q1 (t−1)
{
z 1 2 }|
δys (t) = ys (t) − ys (t − 1) = c1 [xs1 (t) − xs1 (t − 1)] +c2 [xs2 (t) − xs2 (t − 1)]
| {z }
|{z}
c1 xs1 (t)+c2 xs2 (t)+r(t)
c1 xs1 (t−1)+c2 xs2 (t−1)+r(t−1)
+r(t) − r(t − 1)
= c1 a21 xs2 (t − 1) + c1 q1 (t − 1) + c2 xs2 (t) −c2 xs2 (t − 1) + r(t) − r(t − 1)
| {z }
a22 xs2 (t−1)+q2 (t−1)
= (c1 a21 + c2 a22 − c2 ) xs2 (t − 1) + c1 q1 (t − 1) + c2 q2 (t − 1) + r(t) − r(t − 1) =
|
{z
}
α
= αxs2 (t − 1) + c1 q1 (t − 1) + c2 q2 (t − 1) + r(t) − r(t − 1),
(134)
vemos que δys (t) é estacionário pois é uma combinação linear de instâncias de xs2 (t), q1 (t)
q2 (t) e r(t) e estes sinais são todos estacionários.4 Assim, podemos estimar a covariância
de δys (t) a partir duma realização, e estimar os parâmetros do ruı́do que necessitamos para
implementar o previsor e o filtro de Kalman a partir dessas estimativas. Notemos que, como
assintoticamente ȳ(t) difere de ys (t) apenas a menos duma constante, então, ao fim de algum
tempo δȳ (t) → δys (t) pelo que podemos, após esse intervalo de tempo, aproximar δys (t) por
δȳ (t).
Vejamos então o que é que podemos extrair da sequência de auto-correlação de δys (t)
4
∞
,
−∞
q1 (t), q2 (t) e r(t) são ruı́do branco que por definição é estacionário; xs2 (t) é estacionário porque xs2 (t +
1) = a22 xs2 (t) + q2 (t) e |a22 | < 1, logo, xs2 (t) é a saı́da dum sistema estável excitado por ruı́do branco e,
consequentemente, é estacionário.
56
Filtro de Kalman
que designaremos por λδys (τ ). Para τ = 0 temos:
λδys (0) = E [αx2 (t − 1) + c1 q1 (t − 1) + c2 q2 (t − 1) + r(t) − r(t − 1)]2
= α2 E x22 (t) +c21 E q12 (t − 1) +c22 E q2 (t − 1)2 + E r 2 (t)
|
| {z }
|
{z
}
{z
} | {z }
Q11
Q22
R
P
(0)
2
+ E r 2 (t − 1) +c1 c2 E {q1 (t − 1)q2 (t − 1)} −c1 E {q1 (t − 1)r(t − 1)} (135)
|
|
{z
}
{z
}
{z
}
|
Q12
R
S1
−c2 E {q2 (t − 1)r(t − 1)}
|
{z
}
S2
= α2 P2 (0) + c21 Q11 + c22 Q22 + 2R + c1 c2 Q12 − c1 S1 − c2 S2 ,
pois, pelo facto de q1 (t) q2 (t) r(t) ser ruı́do branco, E {q1 (t − 1)r(t)} E {q2 (t − 1)r(t)} =
E {r(t − 1)r(t)} = 0 e xs (t) é independente de q(t) e r(t), pelo que E {xs2 (t − 1)q1 (t − 1)} =
E {xs2 (t − 1)q2 (t − 1)} = E {xs2 (t − 1)r(t − 1)} = E {xs2 (t − 1)r(t)} = 0. Como
xs2 (t + 1) = a2,2 xs2 (t) + q2 (t),
(136)
e da estacionaridade de xs2 (t),
P2 (0) = a222 P2 (0) + Q2 ⇔ P2 (0) =
Q22
.
1 − a222
(137)
Utilizando esta equação em (135),
α2
2
λδys (0) =
+ c2 Q22 + c21 Q11 + 2R + c1 c2 Q12 − c1 S1 − c2 S2
2
1 − a22
(138)
Para τ = 1,
λδys (1) = E {[αxs2 (t − 1) + c1 q1 (t − 1) + c2 q2 (t − 1) + r(t) − r(t − 1)]
[αxs2 (t − 2) + c1 q1 (t − 2) + c2 q2 (t − 2) + r(t − 1) − r(t − 2)]}
2
= α E {xs2 (t − 1)xs2 (t − 2)} +αc1 E {xs2 (t − 1)q1 (t − 2)}
{z
}
|
P2 (1)
+αc2 E {xs2 (t − 1)q2 (t − 2)} − αE {xs2 (t − 1)r(t − 2)} +c1 E {q1 (t − 1)r(t − 1)} +c2 E {q2 (t − 1)r(t − 1)} − E r 2 (t − 1) .
|
|
{z
}
{z
} |
{z
}
S1
De (136),
(139)
S2
R
P2 (1) = E {[a22 xs2 (t − 2) + q2 (t − 2)] xs2 (t − 2)}
a22
= a22 E x2s2 (t − 2) + E {q2 (t − 2)xs2 (t − 2)} = a22 P2 (0) =
Q2 ;
|
{z
}
1 − a222 2
|
{z
}
P2 (0)
0
(140)
57
Filtro de Kalman
E {xs2 (t − 1)q1 (t − 2)} = E {[a22 xs2 (t − 2) + q2 (t − 2)] q1 (t − 2)}
(141)
= a22 E {xs2 (t − 2)q1 (t − 2)} + E {q1 (t − 2)q2 (t − 2)} = Q12 ;
|
{z
} |
{z
}
0
Q12
E {xs2 (t − 1)q2 (t − 2)} = E {[a22 xs2 (t − 2) + q2 (t − 2)] q2 (t − 2)}
= a22 E {xs2 (t − 2)q2 (t − 2)} + E q22 (t − 2) = Q22 ;
|
{z
} |
{z
}
0
(142)
Q22
E {xs2 (t − 1)r(t − 2)} = E {[a22 xs2 (t − 2) + q2 (t − 2)] r(t − 2)}
(143)
= a22 E {xs2 (t − 2)r(t − 2)} + E {q2 (t − 2)r(t − 2)} = S2 .
{z
} |
{z
}
|
0
S2
Utilizando estes valores em (139) obtemos
a22 α
λδys (1) = α
+ c2 Q22 + αc1 Q12 + c1 S1 + (c2 − α) S2 − R.
1 − a222
(144)
Para τ > 1 ,
λδys (τ ) = E {[αxs2 (t − 1) + c1 q1 (t − 1) + c2 q2 (t − 1) + r(t) − r(t − 1)]
[αxs2 (t − 1 − τ ) + c1 q1 (t − 1 − τ ) + c2 q2 (t − 1 − τ ) + r(t − τ )
−r(t − 1 − τ )]}
2
= α E {xs2 (t − 1)xs2 (t − 1 − τ )} +αc1 E {xs2 (t − 1)q1 (t − 1 − τ )}
{z
}
|
(145)
P2 (τ )
+αc2 E {xs2 (t − 1)q2 (t − 1 − τ )} + αE {xs2 (t − 1)r(t − τ )}
−αE {xs2 (t − 1)r(t − 1 − τ )} .
De (139)
xs2 (t − 1) = aτ22 xs2 (t − 1 − τ ) + a2τ 2−1 q2 (t − 1 − τ ) + a2τ 2−1 q2 (t − τ ) + · · · + q2 (t − 2),
pelo que
P (τ ) = aτ22 P (0) =
aτ22
Q2 ;
1 − a222 2
(146)
E {xs2 (t − 1)q1 (t − 1 − τ )} = a2τ 2−1 E {q2 (t − 1 − τ )q1 (t − 1 − τ )} = a2τ 2−1 Q12 ;
{z
}
|
(147)
Q1
E {xs2 (t − 1)q2 (t − 1 − τ )} =
a2τ 2−1
E {xs2 (t − 1)r(t − τ )} =
a2τ 2−2
E {xs2 (t − 1)r(t − 1 − τ )} =
a2τ 2−1
2
2
E q2 (t − 1 − τ ) = a2τ 2−1 Q22 ;
|
{z
}
(148)
Q22
E {q2 (t − τ )r(t − τ )} = a2τ 2−2 S2 ;
|
{z
}
(149)
S2
E {q2 (t − 1 − τ )r(t − 1 − τ )} = a2τ 2−1 S2 ,
{z
}
|
S2
(150)
Filtro de Kalman
58
sendo, para τ > 1,
λδys (τ ) =
a2τ 2−2
a22 α
a22 α
+ c2 Q22 + a22 αc1 Q12 + α (1 − a22 α) S2 .
1 − a222
(151)
Daqui vemos que as equações de λδys (τ ), para τ = 3, 4, . . . , são linearmente dependentes
da equação para τ = 2. Temos, assim, 3 equações linearmente independentes, ou seja, as
equações de λδys (τ ) para τ = 0, 1, 2. Como temos 6 incógnitas este número equações independentes é insuficiente. A única hipótese de encontrar mais equações independentes seria
recorrer a sequências de covariância de diferenciais de ordem superior. Só que vimos anteriormente que estas sequências são linearmente dependentes das do diferencial de primeira
ordem que temos vindo a considerar. Deste modo, só podemos contar com 3 equações, não
sendo possı́vel estimar todos os parâmetros do ruı́do. Felizmente, tal como foi referido anteriormente, os ruı́dos de estado e de medição são quase sempre independentes, pelo que,
nestas situações, sabemos à priori que S1 = S2 = 0, o que reduz o número de incógnitas para
4. No entanto, com S1 = S2 = 0, a equação (151), de λδys (τ ) para τ = 2, pode ser obtida
multiplicando a equação (144) por a22 . Isto significa que estas equações são lineramente
dependentes e que ficamos apenas com 2 equações para determinar 4 parâmetros: R, Q11 ,
Q12 e Q22 . Assim, temos que ter sempre um conhecimento à priori do ruı́do de processo (pelo
menos de um dos elementos da sua matriz covariância). Afortunadamente isto acontece frequentemente, ou porque o ruı́do da equação de estado reponsável o pólo em −1 + j0 é nulo,
ou porque as duas componentes do ruı́do de estado são linearmente dependentes (covariância
do ruı́do de estado tem caraterı́stica 1). Nestas situações as incógnitas reduzem-se a um dos
elementos da matriz Q e á variância R do ruı́do de medição, podendo ser determinadas a
partir das estimativas da sequência de auto-covariância do sinal diferencial δys (t) que, como
é estacionário e érgódico, podem ser estimadas a partir duma única realização deste sinal.
Exemplo 9:
Consideremos de novo o sistema (6), os dados de entrada-saı́da utilizados nos exemplos
4-6, e suponhamos que sabemos que o erro de medição é independente do ruı́do de estado
e que Q11 = 0 (e que consequentemente Q12 também é nulo). A figura 33 mostra o sinal
δȳ (t) = ȳ(t) − ȳ(t − 1) com ȳ(t) definido na equação (132). Podemos ver que se trata dum
sinal estacionário. Estimámos os 7 primeiros elementos da sequência de autocorrelação de
59
Filtro de Kalman
300
δȳ (t) = ȳ(t) − ȳ(t − 1)
200
100
0
−100
−200
−300
0
100
200
300
400
500
600
700
800
900
1000
Tempo
Figura 33: Sinal δȳ (t) = ȳ(t) − ȳ(t − 1).
δys (t) a partir deste sinal depois de retiradas as primeiras 20 amostras, ou seja, calculamos
1000
X
1
δȳ (t)δȳ (t − τ )
λ̂ys (0) =
980 − τ t=21+τ
para τ = 0, , 1, . . . , 6. A tabela 3 compara os valores estimados com os valores teóricos.
Tabela 13: Valores teóricos e estimados de λys (τ ), τ = 0, 1, . . . , 6.
Teórico
Estimado
Erro abs
τ =0
τ =1 τ =2
5021.1 -2481.1 17.05
5463.4 -2881.4 305.9
442.3
400.3 288.8
λys (τ )
τ =3
15.3
-42.9
58.20
τ =4
13.8
36.7
22.8
τ =5 τ =6
12.4
11.2
-50.8 85.9
63.2
74.7
Podemos observar que o erro absoluto descresce até τ = 4 começando a crescer a partir daı́.5
Na tabela 14 podemos ver estimativas de mı́nimos quadrados de Q e R calculadas a partir de
T
Λ̂τmax = λ̂ys (0) · · · λ̂ys (τmax )
para τmax = 1, . . . , 6. Com S1 = S2 = Q11 = Q12 a equações (135), (144) e (151) para
5
Note-se que o mesmo não acontece com o erro relativo pois os valores absolutos de λys (τ ) são muito
mais pequenos para τ > 1.
60
Filtro de Kalman
Tabela 14: Valores estimados de Q e R partir de [λ̂ys (0) · · · λ̂ys (τmax )]T .
Valor teórico
4
Q22
Erro (%)
R
Erro (%)
τmax = 1
-20.03
608
2785.2
11.4
2500
τmax = 2
6.85
71
2753.7
10.1
τmax = 3 τmax = 4 τmax = 5
3.37
4.35
2.31
15.7
8.8
42
2757.7
2756.6
2759.0
10.3
10.3
10.4
τ = 2, . . . , τmax , formam o sistema de equações sobrederterminado.


α2
2
+ c2
2 
2



1 − a22


a
α
2
2

α
+ c2
−1 


2
1 − a22


Q̂


2
2
a22 α
.
Λ̂τmax =  a α
+ c2
0 
22


R̂
2
1 − a22



..
.. 


.

. 
 τmax −1

a22 α
+ c2
0
a22
α
2
1 − a22
τmax = 6
4.41
10.3
2756.5
10.3
Como neste sistema c1 = 1, c2 = 0, e a21 = 0, então α = c1 a21 + c2 a22 − c2 = 1 e o sistema
de equações fica

1
Λ̂τmax
sendo






=






1 − a222
a22
1 − a222
a222
1 − a222
..
.
aτ2max
2
1 − a222

1
1 − a222
a22
1 − a222
a222
1 − a222
..
.
aτ2max
2
1 − a222






Q̂22
=

R̂





2
−1
0
..
.
0
2
−1
0
..
.
0






 Q̂22

 R̂ ,





†






 Λ̂τmax






61
Filtro de Kalman
onde M † designa o pseudo-inverso de M.6
Podemos observar na tabela 14 que alguns erros relativos de Q̂22 são significativamente superiores aos de R. Isto deve-se ao fato do valor verdadeiro de Q22 ser muito mais pequeno que
o de R. Com τmax = 1, (quando o estimador de mı́nimos quadrados é a simples resolução
dum sistema de duas equações a duas incógnitas) Q̂22 é mesmo negativo, o que sendo uma
imposibilidade, obrigou a que o fizéssemos igual a zero no cálculo dos ganhos do previsor e do
filtro de Kalman. O erro de Q̂22 é menor para τmax = 4. Por outro lado, o de R̂ mantem-se
sempre na ordem dos 10%. Utilizando os valores de Q̂22 e R̂, foram calculados os ganhos
Tabela 15: Valores estimados do ganho K do previsor de Kalman.
K1
Erro (%)
K2
Erro (%)
Valor teórico
0.2022
0.0159
τmax = 1
0.0120
94.08
0.0000
100.00
τmax = 2
0.2336
15.55
0.0210
31.73
τmax = 3 τmax = 4 τmax = 5
0.1848
0.2013
1623
8.61
0.45
19.70
0.0134
0.0158
0.0104
15.84
0.86
34.39
τmax = 6
0.2012
0.06
0.0159
0.05
do previsor e do Kiltro de Kalman que se apresentam nas tabelas 15 e 16. A não ser para
Tabela 16: Valores estimados do ganho Kf do filtro de Kalman.
Kf 1
Erro (%)
Kf 2
Erro (%)
Valor teórico
0.1845
0.0177
τmax = 1
0.0120
93.51
0.0000
100.00
τmax = 2
0.2103
14.00
0.0233
31.73
τmax = 3 τmax = 4 τmax = 5
0.1699
0.1838
0.1508
7.92
0.41
18.29
0.0149
0.0175
0.0116
15.84
0.86
34.39
τmax = 6
0.1845
0.01
0.0177
-0.02
τmax = 1 os valores dos ganhos parecem ser muito razoáveis (os erro relativos mais elevados
de K2 e Kf2 podem ser explicados pelo menor valor absoluto de K2 ), sendo mesmo muito bons
nos que foram estimados com τmax = 4 e τmax = 6 onde o erro é praticamente nulo. Para
ilustrar o desempenho destes previsores e filtros de Kalman foi gerado um novo conjunto de
dados com a mesma entrada, e ruı́dos com a mesma covariância. Utlizaram-se os previsores
e filtros teóricos e dimensionados a partir das covariâncias estimadas com τmax = 2, . . . , 6,
e o observador 1 do exemplo 2 para estimar x(t). As tabelas 18 e 19 mostram os valores
6
Aqui M tem mais linhas do que colunas e é de caraterı́stica completa pelo que M † = M T M
−1
MT.
62
Filtro de Kalman
Tabela 17: Valores eficazes dos erros dos previsores de Kalman e do observador 1.
x̃1rms
x̃1rms
Teórico
22.33
4.05
Previsor de Kalman
τmax = 2 τmax = 3 τmax = 4 τmax = 5
22.68
22.43
22.33
22.98
4.05
4.06
4.05
4.10
τmax = 6
22.33
4.05
Obsv 1
23.27
4.16
eficazes dos erros das estimativas. Podemos observar que os desempenhos de dos os previsores de Kalman estimados são superiores superior ao do observador 1 e que, duma forma
geral, estão mais próximos do do previsor teórico. Os desempenhos dos que foram estimados
com τmax = 4 e τmax = 6 são mesmo equivalentes ao do previsor teórico. Como seria de
esperar, os desempenhos dos filtros são superiores ao de todos os previsores. Tal como para
Tabela 18: Valores eficazes dos erros dos previsores de Kalman e do observador 1.
x̃1rms
x̃1rms
Teórico
19.92
3.91
τmax = 2
20.24
3.93
Filtro de Kalman
τmax = 3 τmax = 4 τmax = 5
20.01
19.92
20.54
3.93
3.91
3.97
τmax = 6
19.92
3.91
os previsores, os filtros estimados com τmax = 4 e τmax = 6 têm um desempenho equivalente
ao do filtro teórico.
6.2.3
Sistemas com dois ou mais Integradores
Os sistemas instáveis que acabámos de analisar tinham um só integrador. O sinal estacionário
que permitiu a estimação das covariãncias do ruı́do foi obtido através da diferenciação do
sinal da saı́da. Esta solução também pode ser adotada em sistemas com ni integradores, só
que terão que ser feitas ni diferenciações do sinal de sáida, pois apenas uma não é suficiente
para se obter um sinal estacionário. Como exemplo ilustrativo consideremos o seguinte
sistema estocástico:

 x1 (t + 1) = x1 (t) + a21 x2 (t) + q1 (t)
x2 (t + 1) = x2 (t) + q2 (t)

y(t) = c1 x1 (t) + c2 x2 (t) + r(t)
(152)
63
Filtro de Kalman
onde q1 (t) q2 (t) r(t) é ruı́do branco de média nula com covariância

 


Q11 Q12 S1
 q1 (t) 
E = q2 (t) q1 (t) q2 (t) r(t)
= Q12 Q22 S2  .


r(t)
S1 S2 R
Este modelo pode ser utlizado para estimar a velocidade e a posição dum objeto que se
desloca com uma aceleração aleatória. O diferencial de y(t) é
δy (t) = y(t) − y(t − 1) = c1 [x1 (t) − x1 (t − 1)] +c2 [x2 (t) − x2 (t − 1)] +r(t) − r(t − 1)
|
|
{z
}
{z
}
a21 x2 (t−1)+q1 (t−1)
q2 (t−1)
= c1 a21 x2 (t − 1) + c1 q1 (t − 1) + c2 q2 (t − 1) + r(t) − r(t − 1).
(153)
Podemos ver, na segunda equação de (152), que x2 (t) é o integral do ruı́do branco q2 (t),
sendo, por isso, um sinal não estacionário.7 Como uma das parcelas de δy (t) é proporcional
a x2 (t − 1), este diferencial não pode ser um sinal estacionário. Calculemos agora o duplo
diferencial de y(t):
δy2 (t) = δy (t) − δy (t − 1)
= c1 a21 x2 (t − 1) + c1 q1 (t − 1) + c2 q2 (t − 1) + r(t) − r(t − 1)
−c1 a21 x2 (t − 2) − c1 q1 (t − 2) − c2 q2 (t − 2) − r(t − 1) + r(t − 2)
= c1 a21 [x2 (t − 1) − x2 (t − 2)] +c1 q(t − 1) + c2 q2 (t − 1) + r(t) − 2r(t − 1)
|
{z
}
(154)
q2 (t−2)
−c1 q1 (t − 2) − c2 q2 (t − 2) + r(t − 2)
= −c1 q1 (t − 2) + (c1 a21 − c2 ) q2 (t − 2) + r(t − 2) + c1 q1 (t − 1) + c2 q2 (t − 1)
−2r(t − 1) + r(t).
Como δy2 (t) é uma combinação linear de instâncias de ruı́do branco, é um sinal estacionário
ergódico, e sua sequência de autocorrelação pode ser estimada a partir duma única realização.
7
x2 (t) é um paseio aleatório
64
Filtro de Kalman
Como
λδy2 (0) = E
n
2
δy2
(t)
o
= 2c21 Q11 + 2c1 (2c2 − a21 ) Q12 + 2c22 − 2c1 c2 a21 + c21 a221 Q22 + 6R
(155)
= −c21 Q11 + c1 (c1 a21 − 2c2 ) Q12 + c2 (c1 a21 − c2 ) Q22 − 4R + 4c1 S1
(156)
−6c1 S1 + 2 (c1 a21 − 3c2 ) S2
λδy2 (1) = E δy2 (t)δy2 (t − 1)
+ (2c2 − c1 a21 ) S2
λδy2 (2) = E δy2 (t)δy2 (t − 2) = R + c1 S1 + (c1 a21 − c2 ) S2
τ >2
λδy2 (τ ) = E δy2 (t)δy2 (t − τ ) = 0,
(157)
(158)
(159)
dispomos apenas de 3 equações para determinar 6 incógnitas. Se considerarmos que os
ruı́dos de processo são independentes (S1 = S2 = 0) reduzimos o número de incógnitas.
Nestas condições a equação (157) simplifica-se para
λδu2 (2) = R
(160)
e Q11 , Q12 e Q22 têm que ser calculados a partir de
e
λδy2 (0) − 6R = 2c21 Q11 + 2c1 (2c2 − a21 ) Q12 + 2c22 − 2c1 c2 a21 + c21 a221 Q22
(161)
λδu2 (1) + 4R = −c12 Q11 + c1 (c1 a21 − 2c2 ) Q12 + c2 (c1 a21 − c2 ) Q22
(162)
Para resolver este sistema temos que ter algum conheciemnto à priori sobre q1 (t) ou q2 (t).
Normalmente ocorre uma destas situações:
• qi (t) = 0, i = 1 ou (exclusivo) i = 2, pelo que Qii = Q12 = 0, e ficamos com um sistema
de 2 equações a uma incógnita que pode ser resolvido no sentido dos mı́nimos quadrados.
• q1 (t) e q2 (t) estão linearmente relacionados, i.e., q1 (t) = Kq q2 (t) o que implica Q11 = Kq2 Q22
e Q12 = Kq Q22 reduzindo o número de incógnitas para 2 (Q22 e Kq ).
• q1 (t) e q2 (t) são independentes, sendo Q12 = 0, e só é necessário determinar Q11 e Q22 .
65
Filtro de Kalman
Exemplo 10 (Seguimento duma rampa):
A figura 34 mostra a trajetória perturbada por ruı́do dum corpo a deslocar-se com velocidade
constante. Utilizando um previsor e um filtro de Kalman, pretendemos seguir esta trajetória
e estimar a sua velocidade. Para começar, podemos considerar que y(t) é a saı́da do sistema
1200
y(t)
1000
800
600
400
200
0
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 34: Rampa y(t).
x1 (t + 1) = x1 (t) + x2 (t) + q1 (t)
x2 (t + 1) = x2 (t) + q2 (t)
y(t) = x1 (t) + r(t),
ou seja, a saı́da dum duplo integrador, atuado por uma acelaração aleatória (ruı́do branco
q2 (t)), e com a velocidade também perturbada por ruı́do branco (q1 (t)). O diagrama de blocos
deste sistema está representado na figura 35. A trajetória que pretendemos seguir é o sinal
x1 (t), e é medida por um sensor, também perturbado por ruı́do branco r(t). A velocidade é
a componente x2 (t) do vetor de estados. Como o erro de medição é independente do ruı́do
de estado, a covariância do ruı́do é
 



Q
Q
0
 q1 (t) 
1
1
1
2
E q2 (t) q1 (t) q2 (t) r(t)
= Q12 Q22 0  .


r(t)
0
0 R
Dado que y(t) não é estacionário, esta matriz foi estimada a partir do seu duplo diferencial
δy2 (t) que é exibido na figura 35. A tabela 19 mostra valores da sequûencia de auto-covariância
66
Filtro de Kalman
q2 (t)
x2 (t)
R
R
+
q1 (t)
x1 (t)
y(t)
+
r(t)
Figura 35: Duplo integrador
60
d2
y (t)
40
20
0
−20
−40
−60
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 36: Rampa y(t).
de δy2 (t) para τ = 0, τ = 1 e τ = 2. Como c1 = a21 = 1 e c2 =0, as equações (155)-(157)
Tabela 19: Valores estimados de λδy2 (τ ).
τ
λδy2 (τ )
0
1
2
453.87 -282.15 52.12
Filtro de Kalman
67
tomam a forma
λδy2 (0) = 2Q11 − 2Q12 + Q22 + 6R
λδy2 (1) = −Q11 + Q12 − 4R
λδy2 (2) = R
Substituindo λδy2 (τ ) por λ̂δy2 (τ ), τ = 0, 1, 2, e somando à primeira equação o dobro da
segunda temos
λ̂δy2 (0) + λ̂δy2 (1) = Q22 − 2R
λ̂δy2 (1) = −Q11 + Q12 − 4R
λ̂δy2 (2) = R,
de onde
Q22 = λ̂δy2 (0) + λ̂δy2 (1) − 2λ̂δy2 (2)
R = λ̂δy2 (2)
−Q11 + Q12 = λ̂δy2 (1) + 4λ̂δy2 (2).
Subsitituindo λ̂δy2 (τ ), τ = 0, 1, 2, pelos valores da Tabela 19 obtemos
Q22 = −6.17
R = 52.13
−Q11 + Q12 = −230.02.
Como Q22 é uma variância (de q2 (t)), não pode ser negativo. Dado o seu baixo valor absoluto
concluı́mos que só pode ser nulo. Isto significa que q2 (t) = 0 e que, consequentemente, x2 (t)
é constante, ou seja, o sistema tem uma velocidade constante, o que faz sentido pois o seu
integral (a saı́da do sistema) é uma rampa. Se q2 (t) = 0 então Q12 = E {q1 (t)q2 (t)} = 0.
Teremos, então, Q11 = 230.02.
Com estes valores de Qij , i = 1, 2, j = 1, 2 e R o previsor e o filtro de Kalman estacionários
têm ganhos
0.6864
K = Kf =
0.0068
68
Filtro de Kalman
1200
x1 (t)
Valor medido
Estimativa do previsor
Estimativa do filtro
1000
800
600
400
200
0
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 37: Estimativas de x1 (t) efetuadas pelo previsor e filtro estacionários de Kalman com
x(0) = 02 .
50
∆x2 (t)
Estimativa do previsor
Estimativa do filtro
40
30
20
10
0
−10
−20
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 38: Diferenças das estimativas de x1 (t) (previsor e filtro estacionários de Kalman com
x(0) = 02 ) relativamente ao valor medido.
A figura 37 compara o valor medido de x1 (t) (a saı́da y(t)) com os valores estimados de pelo
previsor e filtro de Kalman. Apesar de escondido pela escala, parece existir uma diferença
constante (e por isso sistemática) nos valores estimados relativamente ao valor medido. Na
figura 38 esta diferença é mais evidente. Por outro lado, podemos ver na figura 39 as estimativas de x2 (t), que sugerem que este sinal é uma rampa, o não pode ser verdade pois, já
69
Filtro de Kalman
1
x2 (t)
Estimativa do previsor
Estimativa do filtro
0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 39: Estimativas de x2 (t) efetuadas pelo previsor e filtro estacionários de Kalman com
x( 0) = 02 .
tı́nhamos concluı́do após a estimação de Q22 , que teria que ser constante. O que falhou no
projeto do previsor e do filtro de Kalman? Analisemos as equações do previsor de Kalman:
x̂1 (t + 1) = x̂1 (t) + x̂2 (t) + 0.6864ê(t)
x̂2 (t + 1) = x̂2 (t) + 0.0068ê(t)
onde ê = y(t) − x̂1 (t). Estas equações mostram que a previsão x̂2 (t) é a saı́da dum integrador (discreto) cuja entrada é ê(t). Como o ganho deste integrador é muito pequeno, x2 (t)
varia muito lentamente, não tendo tempo para atingir o valor da inclinação da rampa que
é aproximadamente 10. Estas variações lentas resultam do facto de nós termos informado
o previsor e o filtro de Kalman de que o ruı́do q2 (t) é nulo (Q22 = 0). Assim, o previsor
está programado para considerar que os erros entre y(t) e x̂1 (t) não podem ser devido a
variações de x2 (t), limitando-as a um valor muito pequeno através da componente do ganho
que afeta esta variável. Por outras palavras, o previsor estacionário considera que a previsão
da velocidade x2 (t) é sempre correta e, como foi informado que este sinal se mantem constante ao longo do tempo, limita fortemente as suas variações. Como x2 (t) foi inicializado
com um valor muito diferente do verdadeiro, o filtro não funcionou convenientemente, não
sendo capaz de estimar este sinal. Para resolver esta situação temos que informar o filtro
que as estimativas iniciais de x1 (t) e x2 (t) são pouco fiáveis. Isso consegue-se substituindo
70
Filtro de Kalman
o previsor estacionário por um não estacionário, em que, nos instantes iniciais, a matriz de
covarância do erro x̃(t) = x(t) − x̂(t) tem valor elevado. Na figura 40 podemos observar as
12
x2 (t)
Estimativa do previsor
Estimativa do filtro
10
8
6
4
2
0
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 40: Estimativas de x2 (t) efetuadas pelo previsor e filtro não estacionários de Kalman
inicializados com x(0) = 02 e P̃ (0) = 100I2 .
estimativas previsor e do filtro de Kalman não estacionários, em que o previsor foi iniciali
T
zado com x̂ = 02 e P̃ (0) = 100I2 . O Previsor arrancou com um ganho K(1) = 0.6573 0
T
passando logo a seguir para K(2) = 1.1841 0.3846 . Assim, no instante t = 1, x̂2 (2) foi
calculado através de
x̂2 (2) = x̂(2) + 0.3846ê(1)
o que corresponde a um integrador com um ganho significativamente maior do que o do
previsor estacionário e que, consequentemente, permitiu que x2 (t) se ajustasse mais rapidamente. Na figura 41 podemos observar que a estimativa x̂2 (t) estabiliza mais ou menos
quando o ganho k2 atinge o seu valor estacionário. Se no instante t = 100 existisse uma
mudança brusca de velocidade, o previsor teria um comportamento idêntico ao do previsor
estacionário, isto é, x̂2 (t) iria variar muito lentamente, pois K2 (t) permaneceria igual ao
valor do filtro estacionário. Para que o filtro fosse mais sensı́vel terı́amos que aumentar o
ganho K2 no instante dessa ocorrência. Isto poderia ser feito através dum reinicialização do
elemento P̃22 (100) da matriz da covariância do erro (variância de x̃2 /100)) para um valor
muito elevado, o que corresponderia a comunicar ao filtro que, nesse instante, a estima-
71
Filtro de Kalman
K2
x2 (t)
K2 (t)
K2 estacionário
x2 (t)
0.4
8
0.3
6
0.2
4
0.1
2
0
0
−0.1
0
10
20
30
40
50
60
70
80
90
−2
100
Tempo
Figura 41: Evolução do ganho K2 (t) e da estimativa de x2 (t) do previsor não estacionário
de Kalman inicializado com x(0) = 02 e P̃ (0) = 100I2 .
tiva da velocidade deixaria de ser fiável. Esta reinicialização poderia ser feita facilmente
se existisse informação à priori que no instante t = 100 iria haver uma variação brusca
na velocidade x2 (t). Caso esta informação não exista, a variação de velocidade tem que
ser detetada através da monitorização do erro ê(t). Sempre que este erro fique fora dum
intervalo [−M, M], M > 0, considera-se que houve uma variação brusca de velocidade e
procede-se à reinicialização da matriz de covariância do erro. O limite do intervalo M pode
ser proporcional ao desvio padrão do erro ê(t),8
q
q
T
σê = C P̃ (t)C + R = P̃11 (t) + R.
Caso não haja variações de velocidade e se o ruı́do for gaussiano, M = 2σê corresponde a
uma média de cerca de 1 reinicialização em cada 20 segundos. Se aumentarmos M para
3σê , o intervalo entre reinicializações passa para cerca de 370 segundos em média. As consequências duma reinicilaização não provocada por uma variação brusca de velocidade não
são muito graves pois limitam-se a tornar as estimativas de dessa mesma velocidade um
pouco mais sensı́vel ao ruı́do de medição durante um perı́odo limitado de tempo.
8
A fi-
O erro ê(t) = y(t) −
r(t)
C x̂(t) = Cx(t) + r(t) − C x̂(t) = C x̃(t) + r(t). Como
é independente de x̂(t) a
variância de ê(t) é CE x̃(t)x̃T (t) C T + R) = C P̃ (t)C T + R. Como C = 1 0 , C P̃ (t)C T é igual elemento
linha e da primeira coluna de P̃ (t), ou seja, P̃11 (t) (C P̃ (t) é igual à primeira linha de P ˜(t) e
hda primeira
i
C P̃ (t) C T é igual à primeira coluna de C P̃ (t), isto é, P̃11 ).
72
Filtro de Kalman
1200
x1 (t)
Valor medido
Estimativa do previsor
Estimativa do filtro
1000
800
600
400
200
0
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 42: Estimativas de x1 (t) efetuadas pelo previsor e filtro não estacionários de Kalman
inicializados com x(0) = 02 e P̃ (0) = 100I2 .
30
Estimativa do previsor
Estimativa do filtro
∆x2 (t)
20
10
0
−10
−20
−30
0
10
20
30
40
50
60
70
80
90
100
Tempo
Figura 43: Diferenças das estimativas de x1 (t) (previsor e filtro não estacionários de Kalman
com x(0) = 02 e P̃ (0) = 100I2 ) relativamente ao valor medido.
gura 42 mostra as estimativas de x1 (t) efetuadas pelo previsor e pelo filtro de Kalman não
estacionários, juntamente com o valor medido deste sinal. Podemos observar que o enviezamento existente no filtro estacionário desapareceu. Isto também pode ser observado na
figura 43, que mostra os desvios das estiatimativas deste filtro e deste previsor de Kalman
relativamente ao valor medido de x1 (t).
73
Filtro de Kalman
A
- Matrizes (semi)definidas positivas e negativas
Definição A.1 - Matriz definida positiva:
Seja M ∈ IRn×n uma matriz simétrica, isto é, uma matriz tal que M T = M. Diz-se que
M é definida positiva e designa-se esse facto por M > 0n×n ou M ≻ 0n×n , se e só
z T Mz > 0,
∀z ∈ IRn tal que z 6= 0n .
Definição A.2 - Matriz semidefinida positiva:
Seja M ∈ IRn×n uma matriz simétrica, isto é, uma matriz tal que M T = M. Diz-se que
M é semidefinida positiva e designa-se esse facto por M ≥ 0n×n ou M 0n×n , se e só
z T Mz ≥ 0,
∀z ∈ IRn tal que z 6= 0n e existir pelo menos um z para o qual z T Mz = 0.
Definição A.3 - Matriz definida negativa:
Seja M ∈ IRn×n uma matriz simétrica. Diz-se que M é definida negativa e designa-se
esse facto por M < 0n×n ou M ≺ 0n×n , se e só z T Mz < 0,
∀z ∈ IRn tal que z 6= 0n .
Definição A.4 - Matriz semidefinida negativa:
Seja M ∈ IRn×n uma matriz simétrica, isto é, uma matriz tal que M T = M. Diz-se que
M é semidefinida negativa e designa-se esse facto por M ≤ 0n×n ou M 0n×n , se e só
z T Mz ≤ 0,
∀z ∈ IRn tal que z 6= 0n e existir pelo menos um z para o qual z T Mz = 0.
Algumas Propriedades das matrizes (semi)definidas positivas e negativas:
1. Uma matriz simétrica M ∈ IRn×n é definida positiva (negativa) se e só se todos os seus
valores próprios forem positivos (negativos).
2. Uma matriz simétrica M ∈ IRn×n é semidefinida positiva (negativa) se e só se tiver, pelo
menos, um valor próprio nulo e todos os outros não negativos (não positivos).
3. As matrizes definidas positivas (negativas) são não singulares:
M ≻ 0n×n ou M ≺ 0n×n ⇒ det(M) 6= 0
4. As matrizes semidefinidas positivas (negativas) são singulares:
M 0n×n ou M 0n×n ⇒ det(M) = 0
Filtro de Kalman
74
5. Se M1 ≻ 0n×n e M2 0n×n então M1 + M2 ≻ 0n×n
Demonstração:
z T (M1 + M2 ) z = z T M1 z + z T M2 z > 0
pois M1 ≻ 0n×n e M2 0n×n e, consequentemente z T M1 z > 0 e z T M2 z ≥ 0 para todo
z 6= 0n .
6. Uma matriz simétrica M ∈ IRn×n é definida positiva (negativa) se e só se todos os seus
valores próprios forem positivos (negativos).
7. Uma matriz simétrica M ∈ IRn×n é semidefinida positiva (negativa) se e só se tiver, pelo
menos, um valor próprio nulo e todos os outros não negativos (não positivos).
Definição A.5
Dadas duas matrizes simétricas M1 e M2 ∈ IRn×n , diz-se que M1 é maior (ou maior
ou igual) do que M2 , designado-se esse facto por M1 ≻ M2 (ou M1 M2 ), se e só se
M1 − M2 ≻ 0n×n (M1 − M2 0n×n ), ou seja, se e só se M1 − M2 for uma matriz (semi)
definida positiva.
Fato A.1
Se M1 e M2 forem matrizes definidas positivas e M1 ≻ M2 (ou M1 M2 ) então kM1 k > kM2 k
(ou kM1 k ≥ kM2 k).