Apêndice B - Dedução de Equações do Filtro de Kalman Estendido

O estado ( x̂k+ ) pode ser estimado através de uma função linear dependente somente

do estado predito ( x̂k− ) e dos valores de medição do sistema ( y k ).

xˆ k+ = K k .xˆ k− + K k . y k (1)

Aplicando o princípio de ortogonalidade entre o erro de estimação dos estados

( xk − xˆ k+ ) e a saída mensurável ( yk ):

[( ) ]
E x k − xˆ k+ . y iT = 0 para i = 1,2,K , k − 1 (2)

Substituindo a equação do estado estimado ( x̂k+ ), temos:

[( ) ]
E x k − K k .xˆ k− − K k . y k . y iT = 0 para i = 1,2,K , k − 1 (3)

Definindo a equação de medição ( yk ), no espaço de estado:

yk = Hk xk +vk (4)

Substituindo a equação de medição ( yk ), temos:

[( ) ]
E x k − K k .xˆ k− − K k .(H k .x k − .v k ) . y iT = 0 (5)

Expandindo a expressão:

[( ) ]
E x k − K k .xˆ k− − K k .H k .x k − K k .v k . yiT = 0 (6)

Rearranjo os termos:

[( ) ]
E x k − K k .xˆ k− − K k .H k .x k . yiT − (K k .v k ). y iT = 0 (7)
 Considerando os ruídos de processo ( wk ) e de medição ( vk ) como variáveis

randômicas e não correlacionadas:

[ ]
E wk . y iT = 0 para i = 1,2, K , k − 1 (8)
E [v . y ] = 0
k
T
i para i = 1,2, K , k − 1 (9)

Cancelando os termos nulos dos ruídos de processo ( wk ) e de medição ( vk ), não

correlacionados, temos:

[( ) ]
E x k − K k .xˆ k− − K k .H k .x k . yiT − (K k .v k ). y iT = 0 (10)

Continuando:

[( ) ]
E x k − K k .xˆ k− − K k .H k .x k . yiT = 0 (11)

Somando e subtraindo o termo ( K k .x k ), temos:

[( ) ]
E x k − K k .xˆ k− − K k .H k .x k + K k .x k − K k .x k . yiT = 0 (12)

Colocando ( K k ) e ( xk ) em evidência nos termos:

[ ( ) ]
E (I − K k .H k − K k ).xk yiT + K k . xk − xˆ k− . yiT = 0 (13)

Onde I é a matriz identidade.

Aplicando a propriedade de adição de termos do operador esperança ( E [ ]):

[ ] [ ( ) ]
E (I − K k .H k − K k ).xk yiT + E K k . xk − xˆ k− . yiT = 0 (14)

Aplicando a propriedade de multiplicação de termos constantes do operador

esperança ( E [ ]):

(I − K k [ ] [( ) ]
.H k − K k ).E xk yiT + K k .E xk − xˆ k− . yiT = 0 (15)
 Aplicar o conceito de ortogonalidade entre o erro de predição dos estados ( xk − xˆ k− )

e a saída mensurável ( yk ), onde:

[(
E x k − xˆ k− . yiT = 0) ] (16)

Eliminando o termo de projeção ( (xk − xˆ k− ). yiT ), temos:

(I − K k [ ] [( ) ]
.H k − K k ).E xk yiT + K k .E xk − xˆ k− . yiT = 0 (17)

Retornando a expressão remanescente contendo ambas as matrizes ( K k ) e ( K k ):

(I − K k [
.H k − K k ).E xk yiT = 0 ] (18)

Através do conhecimento de projeção entre os estados ( xk ) e saídas ( y kT ), não ser

ortogonal:

[ ]
E .xk yiT ≠ 0 (19)

Logo, resulta em uma equação capaz de correlacionar as duas constantes ( K k ) e

( K k ):

(I − K .H k k − Kk ) = 0 (20)

Onde após o rearranjo de termos, determina-se:

K k = I − K k .H k (21)

De posse desta relação entre as matrizes ( K k ) e ( K k ), pode-se retornar a expressão

linear de estimação de estado ( x̂k+ ):

xˆ k+ = K k .xˆ k− + K k . y k (22)

Substituindo a matriz ( K k ):

xˆ k+ = (I − K k .H k ).xˆ k− + K k . y k (23)
 Expandindo a expressão:

xˆ k+ = xˆ k− − K k .H k .xˆ k− + K k . y k (24)

E finalmente, ao se re-alocar os termos, obtém-se a equação de correção dos estados

do processo ( x̂k+ ):

(
xˆ k+ = xˆ k− + K k . yk − H k .xˆ k− ) (25)

Onde a matriz K k é a chamada de Ganho de Kalman. Esta matriz é calculada a cada

instante de tempo k no estimador de estado de Filtro de Kalman Estendido, daí a

importância de sua definição e do conhecimento de sua origem e princípio de cálculo.

A matriz de Ganho de Kalman tem a função de distribuir as disparidades

encontradas entre o valor da variável de saída medida no sistema e a variável de saída
estimada pelo modelo através dos estados preditos. Desta forma faz-se necessário
conceituar o cálculo do erro de predição da variável de saída ( erroy k− ), demonstrado a
seguir:
erroy k− = y k − yˆ k− (26)

Usando as equações de medição ( yk ) e a de predição das saídas ( ŷ k− ), no espaço de

estado, definidas por:

yk = Hk .xk +vk (27)

yˆk− = Hk .xˆk− (28)

Substituindo ( yk ) e ( ŷ k− ) na equação do erro de predição das saídas ( y k − yˆ k− ):

erroy k− = H k .x k + v k − H k .xˆ k− (29)

Colocando a matriz ( H k ) em evidência, temos:

erroy k− = (x k − xˆ k− ).H k + v k (30)

 Definindo o erro de predição dos estados ( x k − xˆ k− ), referente da discrepância entre

o valor real ( xk ) e o valor simulado do modelo ( x k− ), por:

erro k− = (x k − xˆ k− ) (31)

Pode-se expressar o erro de predição das saídas ( y k − yˆ k− ) como:

erroy k− = erro k− .H k + v k (32)

Assumindo o princípio de ortogonalidade entre os erros de estimação dos estados

( xk − xˆ k+ ) e as variáveis de saída, medidas ( ykT ) e preditas pelo modelo ( yˆ k−T ), demonstradas
pelas equações abaixo:

[( ) ]
E x k − xˆ k+ . y kT = 0 (33)
E [(xk − xˆ k+ ). yˆ k−T ] = 0 (34)

Subtraindo estas duas equações:

[( ) ] [(
E xk − xˆ k+ . y kT − E x k − xˆ k+ . yˆ k−T = 0 ) ] (35)

Utilizando a propriedade de subtração, recurso do operador esperança ( E [ ]), pode-

se obter a projeção ortogonal entre os erros de estimação dos estados do processo ( xk − xˆ k+ )

e predição das saídas ( yk − yˆ k− ), como segue abaixo:

[( ) (
E x k − xˆ k+ . y kT − x k − xˆ k+ . yˆ k−T = 0) ] (36)

A qual, uma simples re-alocação de termos, temos:

[( )(
E x k − xˆ k+ . y kT − yˆ k−T = 0 )] (37)

Onde por definição:

(
erro k+ = x k − xˆ k+ ) (38)
erroy −T
k = (y T
k − yˆ −T
k ) (39)
 Logo, temos a projeção:

[
E erro k+ .erroy k−T = 0 ] (40)

Antes de prosseguir a dedutiva para o cálculo do Ganho de Kalman ( K k ) é

conveniente expressar o erro de estimação dos estados ( xk − xˆ k+ ) em função do erro de

predição dos estados ( x k − xˆ k− ), logo:

errok+ = xk − xˆ k+ (41)

Utilizando a equação do estado ( x̂k+ ):

xˆ k+ = xˆ k− + K k .( yk − H k .xˆ k− ) (42)

E substituindo-a na equação do erro de estimação ( xk − xˆ k+ ), temos:

errok+ = xk − xˆ k− − K k .( yk − H k .xˆ k− ) (43)

Indo mais além e substituindo também a equação de medição ( yk ):

yk = Hk .xk +vk (44)

Na equação do erro de estimação dos estados ( xk − xˆ k+ ), temos:

erro k+ = x k − xˆ k− − K k .(H k .x k + v k − H k .xˆ k− ) (45)

Expandindo a expressão:

erro k+ = x k − xˆ k− − K k .H k .x k − K k .v k + K k .H k .xˆ k− (46)

E procurando rearranjar os termos, colocando o erro de predição dos estados

( x k − xˆ k− ) em evidência, temos:

errok+ = (I − K k .H k ).errok− − K k .vk (47)

 De posse então das duas expressões de erros de estimação dos estados ( xk − xˆ k+ ) e de

predição das saídas ( yk − yˆ k− ) em função do erro de predição dos estados ( x k − xˆ k− ):

errok+ = (I − K k .H k ).errok− − K k .vk (48)

−T −T
erroy k = erro .H + v
k
T
k
T
k (49)

Retomando o conceito de ortogonalidade entre estes dois erros:

[
E erro k+ .erroy k−T = 0 ] (50)

Fazendo as substituições das equações dos erros de estimação ( xk − xˆ k+ ) e erros de

predição das saídas ( yk − yˆ k− ):

E [((I − K k )(
. H k ).erro k− − K k .v k . erro k− . H k + v k )
T
]= 0 (51)

Expandindo a expressão:

⎡(I − K k .H k ).errok− .errok−T .H kT − K k .v k .errok−T .H kT +⎤

E⎢ ⎥=0 (52)
⎣ + (I − K k .H k ).errok− .v kT − K k .v k .v kT ⎦

Pelo fato dos ruídos de medição ( vk ) não estarem correlacionados com os erros de

predição dos estados ( x k − xˆ k− ):

(x k − xˆ k− ).v kT = 0 (53)
v k .(x k − xˆ k− ) = 0
T
(54)

Podem-se cancelar estes termos da equação:

⎡(I − K k .H k ).errok− .errok−T .H kT − K k .v k .errok−T .H kT +⎤

E⎢ ⎥=0 (55)
⎣ + (I − K k .H k ).errok− .v kT − K k .v k .v kT ⎦

A equação pode ser reescrita da seguinte forma:

[
E (I − K k .H k ).errok− .errok−T .H kT − K k .vk .vkT = 0 ] (56)
 Aplicando a propriedade de adição de termos do operador esperança ( E [ ]):

[ ] [ ]
E (I − K k .H k ).erro k− .erro k−T .H kT − E K k .v k .v kT = 0 (57)

Aplicando a propriedade de multiplicação de termos constantes do operador

esperança ( E [ ]):

(I − K k .H k ).E [erro k− .erro k−T ].H kT [ ]

− K k .E v k .v kT = 0 (58)

É importante notar que esta última equação apresentada contém um termo de valor
[ ]
esperado para o quadrado do erro de predição dos estados ( E errok− .errok−T ) para o instante

de tempo k . Este termo é definido como sendo a matriz de covariância dos estados Pk− . De

forma semelhante Rk é definida como a matriz de covariância dos ruídos de medição

[ ]
( E v k .v kT ).

[
Pk− = E erro k− .erro k−T ] (59)
[
Rk = E vk .vkT ] (60)

Através destes conceitos de covariância ( Pk− ), pode-se reescrever a equação,

resultando na expressão abaixo:

(I − K k .H k ).Pk− .H kT − K k .Rk =0 (61)

Isolando o termo de K k , pela resolução da equação abaixo, obtém-se a expressão do

Ganho do Filtro de Kalman ( K k ).

(
K k = Pk− .H kT . H k .Pk− .H kT + Rk )−1
(62)

O conceito de Ganho de Kalman é de fundamental para o cálculo da estimação dos

estados. O processo de atualização via Ganho de Kalman é equivalente a um processo de
otimização quadrática sem restrição, como é realizada em algumas outras implementações
de estimadores de estado.
Cálculo da Matriz de Covariância 

A matriz de covariância dos erros de medição ( Pk+ ) pode ser calculada através da
seguinte equação:

[ ] [( )(
Pk+ = E erro k+ .erro k+T = E x k − xˆ k+ . x k − xˆ k+ )]
T
(63)

Onde se pode substituir a equação do erro de estimação dos estados ( xk − xˆ k+ ) em

função do erro de predição dos estados ( x k − xˆ k− ):

erro k+ = (I − K k .H k ).erro k− − K k .v k (64)

Retornando a matriz de covariância ( Pk+ ):

[
Pk+ = E ((I − K k .H k ).erro k− − K k .v k ).((I − K k .H k ).erro k− − K k .v k )
T
] (65)

Expandindo a expressão:

⎡(I − K k .H k ).errok− .errok−T .(I − K k .H k )T − K k .v k .erro k−T .(I − K k .H k )T + ⎤

Pk+ = E ⎢ ⎥ (66)
⎣ − (I − K k .H k ).erro k− .v kT .K kT + K k .v k .v kT .K kT ⎦

Pelo fato dos ruídos de medição ( vk ) não estarem correlacionados com os erros de

predição dos estados ( x k − xˆ k− ):

(x k )
− xˆ k− .v kT = 0 (67)
v k .(x k − xˆ k)
− T
=0 (68)

Podem-se cancelar estes termos da equação:

+
⎡(I − K k .H k ).erro k− .erro k−T .(I − K k .H k )T − K k .v k .erro k−T .(I − K k .H k )T + ⎤
P = E⎢ ⎥ (69)
− (I − K k .H k ).erro k− .v kT .K kT + K k .v k .v kT .K kT
k
⎣ ⎦

A equação pode ser reescrita da seguinte forma:

[
Pk+ = E (I − K k .H k ).errok− .errok−T .(I − K k .H k ) + K k .v k .vkT .K kT
T
] (70)
 Aplicando a propriedade de adição de termos do operador esperança ( E [ ]):

[ ] [
Pk+ = E (I − K k .H k ).errok− .errok−T .(I − K k .H k ) + E K k .v k .vkT .K kT
T
] (71)

Aplicando a propriedade de multiplicação de termos constantes do operador

esperança ( E [ ]):

[ ] [
Pk+ = (I − K k .H k ).E errok− .errok−T .(I − K k .H k ) + K k .E vk .v kT .K kT
T
] (72)

Onde, definem-se as matrizes de covariância:

[
Pk− = E erro k− .erro k−T ] (73)
[
Rk = E vk .vkT ] (74)

Substituindo estas matrizes, e expressando a equação da covariância dos erros de

estimação dos estados ( Pk+ ) em função ma matriz de covariância dos erros de predição dos

estados ( Pk− ), para o mesmo instante de temo ( k ):

Pk+ = (I − K k .H k ).Pk− .(I − K k .H k ) + K k .Rk .K kT

T
(75)

Fazendo uma expansão de termos:

( )
Pk+ = (I − K k .H k ). Pk− − Pk− .K kT .H kT + K k .Rk .K kT (76)

Prosseguindo a expansão de termos:

Pk+ = (I − K k .H k ).Pk− − (I − K k .H k ).Pk− .H kT .K kT + K k .Rk .K kT (77)

Prosseguindo a expansão de termos:

Pk+ = (I − K k .H k ).Pk− − (Pk− .H kT − K k .H k .Pk− .H kT ).K kT + K k .Rk .K kT (78)

Utilizando o conceito da equação e Ganho de Kalman ( K k ):

(
K k = Pk− .H kT . H k .Pk− .H kT + Rk )
−1
(79)
 Rearranjando a equação do Ganho de Kalman ( K k ):

(
Pk− .H kT = K k . H k .Pk− .H kT + Rk ) (80)

Substituindo esta expressão acima de ( Pk− .H kT ) na equação da matriz de covariância

dos erros de estimação dos estados ( Pk+ ):

Pk+ = (I − K k .H k ).Pk− − (K k .(H k .Pk− .H kT + Rk ) − K k .H k .Pk− .H kT ).K kT + K k .Rk .K kT (81)

Expandindo os termos:

( )
Pk+ = (I − K k .H k ).Pk− − K k .H k .Pk− .H kT + K k .Rk − K k .H k .Pk− .H kT .K kT + K k .Rk .K kT (82)

Cancelando os termos idênticos de ( K k .H k .Pk− .H kT ):

( )
Pk+ = (I − K k .H k ).Pk− − K k .H k .Pk− .H kT + K k .Rk − K k .H k .Pk− .H kT .K kT + K k .Rk .K kT (83)

Desta forma pode-se reescrever a equação, em que:

Pk+ = (I − K k .H k ).Pk− − K k .Rk .K kT + K k .Rk .K kT (84)

A qual, pode-se igualmente cancelar o seguinte ( K k .Rk .K kT ):

Pk+ = (I − K k .H k ).Pk− − K k .Rk .K kT + K k .Rk .K kT (85)

Resultando na equação da covariância do erro dos estados estimados ( Pk+ ) em

função da covariância do erro dos estados preditos ( Pk− ), ao instante de tempo k .

Pk+ = (I − K k .H k ).Pk− (86)

Indo mais além, pode-se presumir a matriz de covariância para o instante de tempo
futuro ( k + 1 ) a partir de seu atual ( k ). Para isso, é necessário definir o erro de predição dos
estados para o mesmo instante de tempo futuro ( xk +1 − xˆ k−+1 ):

erro k−+1 = x k +1 − xˆ k−+1 (87)

 Utilizando informações do modelo linearizado, as características dos estados reais
( xk +1 ) e estados preditos ( xˆ k−+1 ), são obtidos do instante de tempo anterior ( k ).

x k +1 = ϕ k .x k + wk (88)
xˆ −
k +1 = ϕ k .xˆ +
k

Assim, pode-se substituir na equação do erro de predição futuro ( xk +1 − xˆ k−+1 ), onde:

erro k−+1 = ϕ k .x k + wk − ϕ k .xˆ k+ (89)

Pela definição de erro de estimação de estados no tempo atual ( x k − xˆ k+ ):

erro k+ = x k − xˆ k+ (90)

Colocando ( ϕ k ) em evidência, pode-se substituir esse conceito do erro de

estimação dos estados no tempo atual ( x k − xˆ k+ ):

erro k−+1 = ϕ k .erro k+ + wk (91)

Utilizando o conceito de matriz de covariância dos erros de predição para o instante

de tempo futuro Pk−+1 , onde:

[
Pk−+1 = E erro k−+1 .erro k−+T1 ] (92)

Substituindo a expressão do erro de predição dos estados no tempo futuro

( xk +1 − xˆ k−+1 ), temos:

[
Pk−+1 = E (ϕ k .erro k+ + wk )(
. ϕ k .erro k+ + wk )
T
] (93)

Expandindo os termos:

− ⎡ϕ k .errok+ .errok+T .ϕ kT + ϕ k .errok+ .wkT +⎤

P k +1 = E⎢ ⎥ (94)
⎣ + wk .errok+T .ϕ kT + wk .wkT ⎦
 Pelo fato dos ruídos de processo ( wk ) não estarem correlacionados com os erros

encontrados após a estimação dos estados ( x k − xˆ k+ ):

(x k )
− xˆ k+ .wkT = 0 (95)
(
wk . x k − xˆ k)
+ T
=0 (96)

Podem-se cancelar estes termos da equação:

⎡ϕ .errok+ .errok+T .ϕ kT + ϕ k .errok+ .wkT + ⎤

Pk−+1 = E ⎢ k ⎥ (97)
⎣ + wk .errok+T .ϕ kT + wk .wkT ⎦

Reescrevendo a expressão resultante:

[
Pk−+1 = E ϕ k .erro k+ .erro k+T .ϕ kT + wk .wkT ] (98)

Aplicando a propriedade de adição de termos do operador esperança ( E [ ]):

[
Pk−+1 = E ϕ k .erro k+ .erro k+T .ϕ kT + wk .wkT ] (99)

Aplicando a propriedade de multiplicação de termos constantes do operador

esperança ( E [ ]):

[ ]
Pk−+1 = ϕ k .E erro k+ .erro k+T .ϕ kT + E wk .wkT [ ] (100)

Onde o valor esperado para o quadrado do erro de estimação dos estados

[ ]
( E errok+ .errok+T ) para o instante de tempo k . Este termo é definido como sendo a matriz

de covariância dos estados Pk+ . De forma semelhante Qk é definida como a matriz de

covariância dos ruídos de processo ( E wk .wkT ). [ ]

[
Pk+ = E erro k+ .erro k+T ] (101)
Qk = E wk .w [ T
k ] (102)
 Retornando ao conceito de matriz de covariância dos erros de predição dos estados
no instante de tempo futuro ( Pk−+1 ):

Pk−+1 = ϕ k .Pk+ .ϕ kT + Qk (103)

Substituindo a covariância dos erros de estimação dos estados:

Pk+ = (I − K k .H k ).Pk− (104)

Logo, temos:

Pk−+1 = ϕ k .((I − K k .H k ).Pk− ).ϕ kT + Qk (105)

Expandindo os termos da equação:

Pk−+1 = ϕ k .(Pk− − K k .H k .Pk− ).ϕ kT + Qk (106)

Continuando a expansão:

Pk−+1 = ϕ k .Pk− .ϕ kT − ϕ k .K k .H k .Pk− .ϕ kT + Qk (107)

Utilizando o conceito da matriz de Ganho de Kalman ( K k ):

(
K k = Pk− .H kT . H k .Pk− .H kT + Rk ) −1
(108)

Substituindo ( K k ) na equação de covariância dos erros de predição dos estados no

instante futuro ( Pk−+1 ), temos:

( (
Pk−+1 = ϕ k .Pk− .ϕ kT − ϕ k . Pk− .H kT . H k .Pk− .H kT + Rk )
−1
).H .P .ϕ
k k
− T
k + Qk (109)

Rearranjando os termos desta equação, chega-se finalmente a equação de cálculo da

matriz de covariância dos erros de predição dos estados no instante de tempo futuro ( Pk−+1 ):

( )(
Pk−+1 = ϕ k .Pk− .ϕ kT − ϕ k .Pk− .H kT . H k .Pk− .H kT + Rk ) .(H
−1
k )
.Pk− .ϕ kT + Qk (110)
 Esta última equação demonstrada é também conhecida como Matriz Dinâmica de
Riccati para sistemas tempo discretos. Esta equação é importante para determinar a
correção dos estados preditos pelo modelo do processo. Uma das vantagens de se utilizar
esta equação é que ela elimina a necessidade do cálculo da atualização da covariância a
cada instante de amostragem uma vez que isto já esta implícito no cálculo de predição.