Desenvolvimento de um Veículo Aéreo Não-Tripulado - LARA ...

espectivamente.
Outro parâmetro associado ao filtro de Kalman é a matriz P, que é a matriz de covariância
do estado e representa a incerteza do filtro a respeito da estimativa. Como o filtro é formulado de
forma recursiva, é preciso fornecer uma estimativa inicial x (0) ∼ N (ˆx (0) , ˆP (0) ).
O filtro pode ser divido em duas etapas, uma de predição (onde somente as informações do
modelo são atualizadas) e uma de correção, (onde somente as medidas são utilizadas). A predição
é dada por:
ˆx (k|k−1) = F (k−1)ˆx (k−1) + G (k) u (k)
ˆP (k|k−1) = F (k−1) ˆP(k−1) F T (k−1) + Q (k)
(5.42a)
(5.42b)
ou seja, a estimativa de estado é atualizada de acordo com o modelo, e a matriz de covariância é
atualizada de acordo com a incerteza de processo Q.
Já a correção é feita utilizado o termo de inovação, que é a diferença entre a medida e a
estimativa de estado dado pela predição, e pelo ganho de Kalman, que leva em consideração a
incerteza da medição e do processo. Assim,
K (k) = ˆP
(
−1
(k|k−1) H T (k) H (k) ˆP(k|k−1) H T (k) (k)) + R (5.43a)
(
)
ˆx (k) = ˆx (k|k−1) + K (k) y (k) − H (k)ˆx (k|k−1) (5.43b)
) (
) T
ˆP (k) =
(I − K (k) H (k) ˆP(k|k−1) I − K (k) H (k) + K(k) R (k) K T (k) (5.43c)
Para modelos não-lineares, a solução mais tradicional é a do Filtro de Kalman Estendido, que
faz uma linearização em torno do ponto de operação para atualizar as matrizes de covariância.
Assim, o modelo é dado por:
x (k) = f(x (k−1) , u (k) ) + w (k)
y (k) = h(x (k) ) + v (k)
(5.44a)
(5.44b)
Linearizando:
(
)
x (k) ≈ f(ˆx (k−1) , u (k) ) + F x x (k−1) − ˆx (k−1) + F u u (k) + w (k) (5.45a)
)
y (k) ≈ h(ˆx (k) ) + H
(x (k−1) − ˆx (k−1) + v (k) (5.45b)
F x = ∂f(x (k−1), u (k) )
∂x (k−1)
F u = ∂f(x (k−1), u (k) )
∂u (k)
H = ∂h(x (k))
∂x (k)
(5.45c)
(5.45d)
(5.45e)
101