Geodesia. Cartografía. Sistemas de referencia. Tiempos.

Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Ejemplo 1-D del filtro de Kalman III
Por tanto las ecuaciones del filtro de Kalman dirán, para cada
instante de tiempo tk+1:
» δˆx − (tk+1)
δˆv − (t k+1)
–
P −
k+1
=
=
» – » +
1 0,01 δˆx (tk )
0 1 δˆv + –
(tk )
» –
1 0,01
P
0 1
+
»
1 0
k 0,01 1
– »
+ 0,1
0
0,01
– ˆ 0 0,01 ˜
Si tk+1 = n, es decir, tiene un valor entero, significa que ha
habido medida. Entonces, calcular la ganancia de Kalman:
Kk+1 = P −
»
1
k+1 0
– „ ˆ 0 1 ˜ P −
k+1
» 0
1
– « −1
+ 0,01 .
Tomamos la medida y calculamos la estimación a posteriori:
» δˆx + (tk+1)
δˆv + (t k+1)
–
=
» −
δˆx (tk+1)
δˆv − (tk+1) P +
k+1 =
ˆ
(I − Kk+1 1 0
˜ −
)P
k+1 .
–
+ Kk+1(δz(tk+1) − ˆ 1 0 ˜ » δˆx − (tk+1) δˆv − (tk+1) donde δz(tk+1) = z(tk+1) − Hk+1(ˆx(tk+1) + δˆx − (tk+1)).
Si no hubo medida, entonces simplemente:
» δˆx + (tk+1)
δˆv + (t k+1)
–
=
» δˆx − (tk+1)
δˆv − (t k+1)
–
, P +
k+1 = P−
k+1 .
Actualizamos ˆx(tk+1) = ˆx(tk+1) + δˆx + (tk+1). Iteramos para
los siguientes valores de k.
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Ejemplo 1-D del filtro de Kalman: simulación I
Simulación de la posición (exacta) y medidas:
('!
(!!
"'!
"!!
&'!
&!!
'!
+,-./.,0
123.34-
–
,
21 / 26
! *
! "! #! $!
)
%! &!! &"!
22 / 26
*
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Ejemplo 1-D del filtro de Kalman: simulación II
('!
(!!
"'!
"!!
&'!
&!!
'!
Usando las medidas para estimar la posición, el resultado es
bueno porque el sensor es preciso y el movimiento en x es
lento.
Si intentamos estimar la velocidad con la fórmula
v(tk) = x(tk)−x(tk−1)
se obtiene ) una estimación muy mala:
!
,-./0/-1
234/45.
!'! *
! "! #! $! %! &!! &"!
%
+
$
'
#
(
"
&
!
!&
∆t
637-0/454
6*3.)/2545*43*234/45.
!" *
! "! #! $!
)
%! &!! &"!
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Ejemplo 1-D del filtro de Kalman: simulación III
Comportamiento de la estimación y del error sin filtro de
Kalman:
#!!
'!!
"!!
&!!
! )
! "! #! $!
(
%! &!! &"!
%
$
#
"
!
*+,-.-+/
0,(-12.-+/)30)*+,-.-+/
405+.-323
0,(-12.-+/)30)405+.-323
!" )
! "! #! $!
(
%! &!! &"!
)
)
*
*
23 / 26
24 / 26