Navegao inercial e GNSS

Universidade Federal do ABC

Prof. Leandro Baroni
Atividade 1: Filtro de Kalman

Objetivo: Implementar um filtro de Kalman estendido (FKE) usado para estimar a posio de um
VANT usando medidas de distncia a trs estaes de referncia.
A atividade baseada no artigo de Mao et al. (2007), que descreve uma aplicao do FKE no qual o
filtro usado para estimar a posio de um VANT usando medidas de distncia.
Problema:
O modelo dinmico discreto do VANT dado por:
x k+1 = x k + v k T cos k
y k+1 = y k + v k T sen k
k+1 = k + k T

(1)

p
k+1 = k + ,k T
p
v k+1 = v k + v,k T

onde (x k , y k ) so as coordenadas do VANT no plano horizontal, k a direo do VANT, k a velocidade angular (taxa de variao da direo) e v k a velocidade com relao ao solo. O ndice k representa o instante de tempo t k e T o perodo de amostragem. As sequncias de variveis aleatrias ,k
e v,k so rudos brancos mutuamente independentes com covarincias Q e Q v respectivamente.
Usando o vetor

[
xk = x k

yk

vk

]T

(2)

pode-se escrever o modelo dinmico (1) na forma matricial:

xk+1 = 0

0 0

1 0

T cos k

0
[
]
0
T sen k

,k

xk + 0
0
0 1 T
0
v,k
p

0
0 0 1
0

p
T
0
0 0 0 0
1
{z
} |
{z
}
|

(3)

f (xk )

As medidas so distncias das estaes de referncia ao VANT. A distncia da i -sima estao ao

VANT
z k,i =

(x k X i )2 + (y k Yi )2 +k,i
|
{z
}

(4)

h(xk )

onde (X i , Yi ) so as coordenadas da i -sima estao de referncia e k uma sequncia de variveis

aleatrias de rudo branco. As coordenadas das estaes de referncia, em metros, so:

X 1 = 0 e Y1 = 0
X 2 = 3000 e Y2 = 2000
X 3 = 0 e Y3 = 2000

Dadas a dinmica e a equao de medidas no lineares, uma estimativa da localizao do VANT ao

longo do tempo envolve o uso de um FKE. Este filtro assume que ambas equaes da dinmica e
medida devem ser linearizadas em torno da melhor estimativa disponvel.
Os passos para a implementao do FKE so:
+
Inicializao: De um estado x +
0 e uma covarincia P 0 iniciais em k = 0. Os valores iniciais sugeridos

so:

1

1


+
x 0 = 0

0

0

1000
0
0
0

0
1000 0
0

+
e P0 = 0
0
0.3
0

0
0
0 0.01

(5)

+
Propagao: dados x +
e P k1
calculados no instante k 1, o valor de x
propagado ao instante k
k
k1

calculado por:
x
x+
k = f (
k1 )

(6)

A matriz de derivadas (matriz de transio) :

0
f

(x ) = 0
k1,k =

x k
0

0 vk1 T sen k1

vk1 T cos k1

T cos k1

T sen k1

(7)

A matriz de covarincias propagada

+
P k = k1,k P k1
Tk1,k +GQG T

(8)

onde G definida em (3) e Q a matriz de covarincias dos termos de erros e v . O valor sugerido

Q=

[
0.0003

10

(9)

Clculo da predio das medidas:

(xk X 1 )2 + ( yk Y1 )2

2 + ( y Y )2

zk = h(x
)
=
(
x

X
)

2
2
k
k

2
2
(xk X 3 ) + ( yk Y3 )

(10)

onde h definido em (4). Calcule a matriz de derivadas de h:

x X 1
k

Hk =

zk,1

x
X
2

(x ) = kz
k,2
x k
xk X 3

zk,3

yk Y1

zk,1
yk Y2

zk,2
yk Y3

zk,3

0 0 0

0 0 0

0 0 0

(11)

e a covarincia do erro da predio das medidas

S k = Hk Pk HkT + R

(12)

R covarincia de k , com valor sugerido de

100
0
0

R =
100
0

0
0
0
100

(13)

Atualizao: Calcule o ganho de Kalman:

K k = P k HkT S k1

(14)

Dado o vetor de medidas, atualize a estimativa do estado:

x +
k + K k (z k zk )
k =x

(15)

P k+ = (I K k Hk )P k

(16)

e sua covarincia:

Implementao em MATLAB
Os dados das medidas de distncia das estaes esto em um vetor z, no qual cada linha representa
as medidas de uma estao e cada coluna representa um instante de tempo. H ainda os valores da
trajetria verdadeira, xtrue e ytrue. O perodo de amostragem T 1 s. H vrios conjunto de dados,
escolha um. Para carreg-los no MATLAB, use o comando:

load 'DadosNN.mat'

Atividades:

1. Faa a implementao do filtro de Kalman estendido para a localizao do VANT.

a) Plote os valores estimados dos estados.
b) Calcule e plote o trao da matriz de covarincias P k+ . Obs.: O MATLAB possui a funo trace,
para calcular o trao de uma matriz.
2. Plote os erros de estimao a posteriori para as coordenadas x e y e o desvio padro correspondente no mesmo grfico. Esta uma forma de verificar a qualidade da estimativa quando os
dados reais esto disponveis (dados simulados).
3. Compare sua estimativa com os valores reais fornecidos xtrue e ytrue, plotando nos mesmos
grficos que as estimativas.
4. Modifique o script e repita os passos anteriores para o caso de uma estao ficar inoperante.
5. Discuta o desempenho do filtro nos dois experimentos feitos (com 2 e 3 estaes).

Referncias:
Brown, R. G.; Hwang, P. Y. C. Introduction to random signals and applied Kalman filtering. 3. ed. New
York: John Wiley & Sons, 1996. 480 p.
Grewal, M. S.; Andrews, A. P. Kalman filtering : theory and practice using MATLAB. 3 ed. Hoboken,
USA: Wiley-IEEE Press, 2008.
Mao, G.; Drake, S.; Anderson, B. D. O. Design of a Extended Kalman Filter for UAV Localization. In:
Proceedings of Information, Decision and Control 2007, Adelaide, February 2007. pp. 224-229.
Welch, G.; Bishop, G. An introduction to the Kalman filter. In: 25 International Conference on Computer Graphics and Interative Techniques, 12-17 August 2001. Los Angeles. Proceedings... Los Angeles:
SIGGRAPH, 2001.