Sistemas de Navegación...
30
1 Navegación Integrada • Fusiona datos de diversos instrumentos. – Convergen dos tecnologías de punta: • GPS y mini/micro instrumentos inerciales • Mejora de: calidad, disponibilidad, robustez y continuidad. • Permite complementar: – Sensores con buena respuesta en baja frecuencia – Sensores con buena respuesta en alta frecuencia – Buen desempeño a corto y largo plazo • Casos típicos: – Instrumentos inerciales + Radar + GPS + sensor solar y/o star tracker • http://www-gpsg.mit.edu/~tah/12.540/ – Odómetros + sensor infrarrojo + modelo de vehículo – Instrumentos inerciales y baro-altímetro • Es una aplicación de técnicas de observación/estimación del estado.
Transcript of Sistemas de Navegación...
1
Navegación Integrada
• Fusiona datos de diversos instrumentos.– Convergen dos tecnologías de punta:
• GPS y mini/micro instrumentos inerciales • Mejora de: calidad, disponibilidad, robustez y continuidad.• Permite complementar:
– Sensores con buena respuesta en baja frecuencia – Sensores con buena respuesta en alta frecuencia– Buen desempeño a corto y largo plazo
• Casos típicos:– Instrumentos inerciales + Radar + GPS + sensor solar y/o star tracker
• http://www-gpsg.mit.edu/~tah/12.540/– Odómetros + sensor infrarrojo + modelo de vehículo– Instrumentos inerciales y baro-altímetro
• Es una aplicación de técnicas de observación/estimación del estado.
2
Principio de Estimación /Observación del Estado
1 ,( , ) ; 0; δ+ = + = =
M odelo del procesoT
k k k k k k l k k lx f x u v E v E v v Q
,( , ) ; 0; Tk k k k k k l k k ly h x u E E R= + = =
M od elo d e las m ed icion es
η η η η δ
Modelo
Kk.
Mediciones
MedicionesPredichas
Funciones ForzantesConocidas/Medidas
( , )k kf x u
Proceso?
ˆ( ) k kx k K error∆ =Corrección del estado
erroru
3
Esquema de Navegación Integrada
AlgoritmoINS
Comparación
Errores o innovación
Mediciones externas: hk(x,pe)
Variables de Navegación Estimadas
Algoritmo de cálculo de Correcciones estado y param.
Calibr. de instrum.
Modelo de las mediciones
( )bsm t
( )kx t∆( )i kp t∆ ( )e kp t∆ˆ ( )k ky t
( )k ky t
( )k ky tδ
x
IMU
Movimiento del vehículo x(t)
m
kK
ˆ ˆ( , )k eh x pˆ ip x
Proceso continuono accesible
χ(tk-2) χ(tk-1) χ(tk)
Medidas externas
Medidas inerciales
m(t)χ(t)
( )bsm t ( )b
sm t ( )bsm t
ky1ky −2ky −Diagrama de Tiempos
1 1 1 1 1 1 1
ˆ( , ); ( ) . . , ( )( , , ) (0, ( ) )
( ) ( ( ), ) ; (0, );
k k k x k
i m m m
k k k k e k k k
x f x m t t x t v a x P tm M m p N t Qy t h x t p N Rd+ + + + + + +
= ≥
= ξ ξ δ
= + η η
∼∼
∼
0ˆ; ; ( ) (0, ( ) ); ( ) ( ), ( )
0i i i
k k k p ke e e
p Qp t t t N t p t N p t P t
p Qξ⎡ ⎤ ⎡ ⎤ ⎡ ⎤
= = ≥ ξ δ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ξ⎣ ⎦ ⎣ ⎦ ⎣ ⎦∼ ∼
Modelo cinemático markoviano continuo y mediciones discretas
Mecanización INS continua( ) [ ( ) ( ) ]T T Tt x t p tχ
1
1
1 1 1 1
1 1 1 1 1 1
ˆ ˆ ˆ ˆ ˆ( , ); ( ) ; [ , )
ˆ ˆ ˆ( ) 0; ( ) ; [ , )ˆˆ ˆ ˆ( ) ( ( ), ( ), ( )) ( ( ), ( ),0);
ˆ ˆ ˆ( ) ( ( ), ( ))ˆ( ) ( ) ( ) diferencia obse
k k k k
k k k k
i k m i k
k k k k e k
k k k k k k
x f x m x t x t t t
p t p t p t t t
m t M m t p t t M m t p ty t h x t p ty t y t y t
+
+
+ + + +
+ + + + + +
= = ∈
= = ∈
= ξ =
=
δ − ← rvable
Ecs. Cinemáticas
Modelos Brownianos de los parámetros
Descripción Matemática
4
5
Modelos de error usuales de los sensores inerciales
Sesgos de los sensores inerciales
( ) 3,; 0 cov( ) 0 " "T
o k j k j k kQ si Q bias puro= = ∈ = = ⇒η ξ η η ξ ξ δ
( )( )b b bff I diag s f fδ= + +
Error de factor de escala
: medida de la fuerza específica; : fuerza específica real; : : ruido blanco; :vector del desconocimiento del factor de escala
bfbf
bfδ3
fs ∈( )0 0
0 0
0 0f
x
y
z
sdiag s s
s
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
Error por falta de ortogonalidad de los ejes del cluster de sensores
: apartamiento angular de la ortogonalidadde los ejes del sensor.
6fo ∈( )( )b b b
ff I o f fδ= + Γ +
( )0
0
0f
xy xz
yx xz
zx zy
o o
o o o
o o
⎡ ⎤⎢ ⎥
Γ = ⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
6
Modelo usual de sensores inerciales
m: f , ω
( )
( ) 9
; (0, ( ) )
; (0, ( ) );; (0, ( ) );
;
bm m m m m
m b m b b b
m m
x xy xz
m m m yx y yz m
zx zy z m
I N t Q
N t QN t Qσ σ σ σ
= + Σ + + δ
= + ξ ξ δ
σ = σ + ξ ξ δ
⎡ ⎤σ σ σ⎢ ⎥Σ = Σ σ = σ σ σ σ ∈⎢ ⎥⎢ ⎥σ σ σ⎣ ⎦
m m b ξ ξ
b A bA
∼
∼∼
25[ , , , ]Tf fω ωσ σ ∈ip b b
Corrección de la medición inercial
( ) ˆˆˆ bm mI= + Σ +m m b
Estimación de las variables de navegación
Enfoque Bayesiano I
7
• Supuesta conocida la densidad de distribución condicional a posteriorip(χ(tk)/Yk), Yk=y0, y1,...,yk en tk. del vector conjunto:
• Las ecs. marcovianas (cinemática +modelo de mediciones) permiten, en teoría, propagar la densidad de probabilidad conjunta a priori:
• Con la nueva medición yk+1 , la densidad a posteriori se actualiza según:
( ) [ ( ) ( ) ]T T Tt x t p tχ
1
Ecs. de Fokker-Plank ( ( ) / ( ));
( ( ) / ) ( ( ) / ( )) ( ( ) / ) ( );
Predicción óptima E ( ) / ... ( ( ) / )
k k
k k k k k k
k k k
p t t t t
p t Y p t t p t Y d t t t
t Y p t Y d
−
∞ ∞−
+−∞ −∞
→ χ χ ≥
χ = χ χ χ χ ≥
→ χ = χ χ χ
∫
∫ ∫
11 1
1
1
1
1
1 1
( ( ) / )( ( ) / )
Estimación óptima "a posterior
( / ( )
i" E ( ) / ... ( )
(
/
)
)
)/k k
k kk k
k
k k
k
k
pp t Yp t Y
t
p yy
Y
Y
p d
t
Y
−+
+ +
∞ ∞
+ + +−∞ −∞
+
+
+χχ =
→ χ χ χ
χ
χ = ∫ ∫
8
Deducción de la relación Bayesiana
• Usando relaciones de probabilidad condicional, por un lado se tiene:
• Y por el otro…
• Finalmente igualando (1) y (2) resulta:1
11
11
11( ( ) / )
( ( ) / ) (( /
( ) /))
)/
(,
( )k k
k k kk
k kk
k k
p t Yp t Y p t Y y
p y Yp y t−
++ + +
+ +
+
χ χχ = χ =
1 1 1 1 1
1 1 1
1 1 1 1
1 1 1
1 1
( ( ), ) ( / ( )) ( ( )) Prob. condicional( , / ( )) ( ( ))( / , ( )) ( / ( )) ( ( )) Prob. condicional( / ( )) ( , ( ))
( / ( ))
k k k k k
k k k k
k k k k k k
k k k k
k k
p t Y p Y t p tp y Y t p tp y Y t p Y t p tp y t p Y t Markov
p y t
+ + + + +
+ + +
+ + + +
+ + +
+ +
χ = χ χ ←
= χ χ
= χ χ χ ←
= χ χ ←
= χ 1( ( ) / ) ( ) Prob. condicional (2)k k kp t Y p Y−+χ ←
1 1 1 1 1
1 1 1
1 1 1
( ( ), ) ( ( ) / ) ( ) Prob. condicional( ( ) / ) ( , )( ( ) / ) ( / ) ( ) Prob. condicional (1)
k k k k k
k k k k
k k k k k
p t Y p t Y p Yp t Y p y Yp t Y p y Y p Y
+ + + + +
+ + +
+ + +
χ = χ ←= χ
= χ ←
9
Estimación de las variables de navegación
Enfoque Bayesiano II
• El numerador surge de las relaciones bayesianas:
• Y el denominador de:
1
1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1
1 1
1
( , / ( )) ( / , ( )) ( / ( ))( ( ( )) ) ( ) ( ) ( ( ))
( / ( )) ( ( ( )) ) ( )
( / ( )) (k
k k k k k k k k
k k k k k k k k k
k
k
k k k k k k k k
k kp y
p y t p y t p ty h t p y t h t
p y t y h t
p y
p d
t+
+ + + + + + + +
+ + + + + + + + + +
+ +
+
+ + + + +
+ η
+
+
η χ = η χ η χ= δ − χ − η η ← = χ + η
χ = δ − χ
χ =
− η η η∫1 1 1( ( )))k kh t+ +− χ
1 1 1 1 1
1 1
1 1 1 1 1
1
( , ( ) / ) ( / ( ), ) ( ( ) / )( / ( )) (
( / ) ( /
( ) / )
( )) ( ( ) / ) ( )k k k k
k k k k k k k
k
k
k k
k
k k k
p y Y p
p y t Y p y t Y p t Yp y t p
y t
t
p t Y d t
Y
+ + +
+ + + + +
+
+
+
+
+
χ = χ χ
χ
= χ χ
= χ
χ∫
10
Estimación de las variables de navegación
Soluciones Aproximadas:
• Las Ecs. Fokker Plank sólo tienen solución en muy contados casos!• Dos aproximaciones de 2º orden de gran valor práctico son:
– Extended Kalman Filter (EKF) – Sigma Point Kalman Filter
• Ambas usan– Sólo los momentos de 1º y 2º orden de las distribuciones involucradas.– Un método (distinto en cada caso) de linealización aproximante de las
Ecs. marcovinas.– Una estimador lineal óptimo.
• En caso de Gaussianidad y linealidad ambos métodos son exactos!
11
Estimación Lineal OptimaDados: • Un proceso aleatorio χ(t) con medidas discretas• Su estimador óptimo y covariancia a priori en t: tk < t< tk+1:
El estimador (regresor) lineal óptimo a posteriori resulta:
Corrección a posteriori:
Problema: Propagar en el tiempo
( ) ( ) / ( ) ( )ˆ ( )ˆ ˆ ( ) / ( ) ; ( ( ) ( )) ( ( )) / ( ) ( ) / ( ) ( )ˆ ( )
k x xpTk k
k px p
tE x t Y P t P tx t
E t Y t E t t t Y P tE p t Y P t P tp t
− −−− − − −
χ − −−
−δχ⎡ ⎤⎡ ⎤⎡ ⎤
χ = χ = χ − χ δχ = ⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦
( )( )
1 1 1 1 1 1
1 1 1 1
1
1 1 1
1
1 1 1 1 1
1 1 1
1( )
ˆ ˆ ˆ( ) ( ( ));ˆ ( ) ( ) /
( ) ( )
( ) ( ) ( ) ( ) ( )
( ) ( )
k k k k k k
k k k k k
k y k y k
k k y k y k y k
k k y k
ky t
t K y y t
y t E y t Y
K P t P t
P t P t P t P t P t
P t K P t
− −+ + + + + +
−+ + + +
−− −+ χ + +
−− − − −χ + χ + χ + + χ +
− −χ + + χ +
+δ
χ χ + −
=
=
= −
= −
11 1
1 1 1 1 1 1 11 1
1
1( ) ( )ˆ( ) ˆ ( )ˆ ˆˆ( ( )) ( ) ( )ˆ ( ) ˆ ( )
( )
kk k
k k k k k k i kk k
e k
kt x tx t x t
K y y t t p tp t p t
p t
+ −+− − +
+ + + + + + + −+ +
+
+δχ ∆⎡ ⎤⎡ ⎤⎡ ⎤⎢ ⎥− = χ − χ ∆ = − ⎢ ⎥⎢ ⎥⎢ ⎥ ⎣ ⎦ ⎣ ⎦⎢ ⎥∆⎣ ⎦
1 1 1
1 1 1
1 1 1
1 1 1 1
( ) ( )( ( )) /
( ) ( )( ( )) /
( ) ( ) ( ) /
( ) ( ) ( ) /
Ty k k k k
Ty k k k k
Tk k k k
Tk k k k
P t E t y t Y
P t E y t y t Y
P t E t t Y
P t E t t Y
− −χ + + +
−+ + +
− − −χ + + +
χ + + + +
δχ δ
δ δ
δχ δχ
δχ δχ
0 1 , ,..., k kY y y y=
ˆ ( ) , ( ) , ( ), !!!y yt P t P t P− − − −χ χχ
12
Propagación aproximada del Estimador a Priori:Solución: “Linealización analítica” ó EKF
“Principio de equivalencia cierta” para la propagación de la estimación a priori:Integrar las ecuaciones cinemáticas con cada v.a. sustituida por su valor esperado.
Equivale a integrar el algoritmo de navegación INS en el intervalo [tk, tk+1) con C.I.:
1
1
ˆ ˆ ˆ ˆ ˆ( , ); ( ) ; [ , )ˆ ˆ( ) ( ( ), ( ),0); ( ) ( ) 0
ˆ ˆ ˆ ˆ( ) 0 ( ) ( ) ; [ , )
k k k k
i k
k k k k
x f x m x t x t t t
m t M m t p t t E t
p t p t p t p t t t
− − − −+
− −
− − −+
= = ∈
= ξ → ξ =
= ⇒ = = ∈
Introducimos apartamientos del estado “real” respecto de su estimación a priori:
ˆ ˆ( ) ( ) ( ); ( ) ( ) ( ); ( ) 0; ( ) 0kx t x t x t p t p t p t E x t E p t− − − − − −δ − δ − δ = δ =
Y el modelo dinámico lineal incremental:
1 1 1 1 1 1
ˆ ˆ( , ( , , ) ) ( , ( , ( ),0)ˆ ˆ ˆ ˆ ˆ ˆ( , ( , ( )) ( , ( , ( ))) ( , ( , ( )))
ˆ ˆ ˆ ˆ( ) ( , ; ) ( ) ( , ; ) ( )
i m i k
x i k m i k p i m i k m
k k x e k k p e k e k
x f x M m p f x M m p t
f x M m p t x f x M m p t M p f x M m p t M
p py t h x p t x t h x p t p t
− − −
− − − − − − − −ξ
− − − − − −+ + + + + +
δ = ξ −
δ + δ + ξ
δ = = ξ
δ = δ + δ + 1 1 1; (0, )k k kN Rd+ + +η η ∼
13
Modelo linealizadoRescribiendo el modelo lineal:
0 0 00 0 0 0 0 ( ) ( ) ( )0 0 0 0 0
0 0 ( ) ( ) 0 0
0 0
x m p m m
i i i c c
e e e
m
c i
e
c cF B
x f f M x f Mp p I F t B t tp p I
Qcov t Q t Q
Q
− −
−
−
⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥= + = +⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦
⎡ ⎤⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦
ξ
ξδχ
δ δ ξδ δ ξ δχ ξδ δ ξ
ξ
1
1 1 1 1 1 1 11( ) ; (0, )
k
k x pi pe i k k k k k kkeH
xy h h h p H t N Rd
p+
−
−+ + + + + + ++
δ⎡ ⎤⎡ ⎤δ = δ + η = δχ + η η⎢ ⎥⎣ ⎦ ⎢ ⎥δ⎣ ⎦
∼
ˆ( ) ( ) ˆ ˆ( ) ( ) ( ( ) ( )) , ( ) ( ) ˆ( ) ( )TT
kk
x t x tP t cov t E x t x t p t p tp t p t−
− − −−χ
⎡ ⎤− ⎡ ⎤χ = − −⎢ ⎥ ⎣ ⎦−⎣ ⎦
1 1 1 1 1
1 1 1 1 1 1 1
( ) ( )( ) / ( ) ( )
( ) ( )( ) / ( ) ( ) ( )
T Ty k k k k k k
T Ty k k k k k k k k
P t E t y Y P t H t
P t E y t y Y H t P t H t Rd
− − −χ + + + χ + +
− −+ + + + χ + + +
= δχ δ =
= δ δ = +
Y las covariancias…
( ) 1
1 1 1 1 1 1 1( ) ( ) ( ) ( ) ( )T Tk k k k k k kK P t H t H t P t H t Rd
−− −+ χ + + + χ + + += +
14
Propagación de la matriz de covariancia a priori
( ) ( ) ( ) ( ) ( ) ; ( ) ( )T Tc c c c c k kP F t P P F t B t Q t B t P t P t− − − −
χ χ χ χ χ= + + =
( ) ( , ) ( ) ( , ) ( , ) ( ) ( ) ( , )k
tT T Tk k k c c ct
P t t t P t t t t B Q B t d−χ χ= Φ Φ + Φ τ τ τ Φ τ τ∫
( , ) ( ) ( , ); ( , ) ;k c k k k kt t F t t t t t I t tΦ = Φ Φ = ≥
y ahora: tn= tk+nTs, n=0,…, Mk, MkTs =tk+1-tk
1
1 1 1 1 1 1
1 1 1 0 0
( ) ( , ) ( , ) ( , ) ( ) ( ) ( , ) ;
; ( ); 0,1,... ; ( )
n
n
tT T Tn n n n n n n n c c c nt
Tn n n n n k k k
P t P t t P t t t B Q B t d
P P Q P P t n M P P t
+− − −χ + + + + + +
− −+ + + χ χ
= = Φ Φ + Φ τ τ τ Φ τ τ
= Φ Φ + = = =
∫
( , ) ( ) ( , ); ( , )n c n n nt t F t t t t t IΦ = Φ Φ =
1
1
1 1
21 1
( ) ( ), ( ) ( ); [ , ]( , ) ( ( ) );
( , ) ( ) ( ) ( , ) ; ( ) ( ) ( )n
n
c c n c c n n n
n n n c n s
t T T Tn n c c c n c n c c n s st
B t B t F t F t t t tt t exp F t T
Q t B Q B t d B t Q B t T TOS T+
+
+ +
+ +
∀ ∈ ⇒Φ = Φ
= Φ τ τ τ Φ τ τ +∫
15
Limitaciones el Método EKF
• Las hipótesis centrales del EKF i.e.: – El “principio de equivalencia cierta” para propagar las soluciones.– Covariancias determinadas por el modelo linealizado alrededor de dichas
soluciones.• No se verifican para
– C.I. muy apartadas de las reales, ruidos de alta potencia, sistemasaltamente no lineales, tiempos de propagación elevados
• En ciertos casos esto provoca divergencias del método EKF.
Propagación aproximada del Estimador a Priori:Solución: “Linealización estadística” o por sigma puntos
Denotamos: Fk:tk→tk+1 a la transición
entre 2 instantes sucesivos de las Ecs. estocásticas Markovianas:
Usando: a) las mediciones inerciales tomadas en el intervalo [tk, tk+1], b) una realización del vector de ruidos discreto equivalente:
c) una realización del vector de estados inicial χ(tk).
11
1
( )( ) ( ( ), ; )
( )k
k k k kk
x tt F t m
p t+
++
⎡ ⎤= =⎢ ⎥⎣ ⎦
χ χ ξ
1 1 1 1 1 1
ˆ( , ); ( ) . . , ( )( , , ) (0, ( ) )
( ) ( , ) ; (0, );
k k k x k
i m m m
k k k e k k k
x f x m t t x t v a x P tm M m p N t Qy t h x p N Rd+ + + + + +
= ≥= ξ ξ δ
= + η η
∼∼∼
[ ] (0, )T T T Tn m i e n c sN Q Tξ ξ ξ ξ= ∼
0ˆ; ; ( ) (0, ( ) ); ( ) ( ), ( )
0i i i
k k k p ke e e
p Qp t t t N t p t N p t P t
p Qξ⎡ ⎤ ⎡ ⎤ ⎡ ⎤
= = ≥ ξ δ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ξ⎣ ⎦ ⎣ ⎦ ⎣ ⎦∼ ∼
16
17
Linealización estadística
• En lugar de evaluar momentos de 1º y 2º orden con el modelo local tangente en el instante de inicio del intervalo.
• Usa las imágenes de un conjunto de “σ−puntos” χi definidos sobre un dominio de las v.a.s involucradas.
• Los σ−puntos y r se elijen tal que capten los primeros 2 momentos de la distribución de las v.a.s en tk.
• Se calculan momentos de 1º y 2º orden de las imágenes χ usando promedios ponderados.
11
1
( )( ) ( ( ), ; ) 1,..,
( ) i
ki k k i k k
k i
x tt F t m i r
p t+
++
⎡ ⎤= = =⎢ ⎥⎣ ⎦
χ χ ξ
(.,.; ) .kF m Flujo de las ecs diferenciales cinemáticas≡tk
tk+1
,i i k
puntosen t
−σχ ξ
1
Imágenes de los eni kt +χ
Resumen del algoritmo Sigma Point Kalman Filter (SPKF)
• Inicialización:
• de k=0 hasta fin de navegación 1. Determinación del conjunto de σ-puntos
2. Determinación de pesos de los promedios ponderados de 1° y 2° momentos de la imagen, respectivamente:
3. Propagación temporal (tk+1=tk+Ts) de los σ−puntos mediante el método de integración numérica de las ecuaciones de navegación (algoritmo INS)
0
0
0
( ) 0ˆ ( ); ( )
0 ( )
( ) 0 0ˆˆ ˆ ˆ[ , , ] ; ( ) 0 0
0 0
xo o o
p
To o o o o m
o
P tE t P t
P t
P tP t Q
R
χ
χ
σ
χ χ
σ χ ξ η
⎡ ⎤= = ⎢ ⎥
⎣ ⎦⎡ ⎤⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦
ˆ ˆ[ ] , 0,..., , , 1,..., [ ] [ ] [ ]i ik k k k k k k ki r i rΣ = = = + = = Χ × Ξ × Ησ σ σ δσ
0 0[ ] [ ,..., ]; [ ] [ ,..., ]r rk k k k k kΩ = ϒ =ω ω γ γ
1[ ] ([ ],[ ], )n n n nF m−+Χ = Χ Ξ
18
Resumen del algoritmo Sigma Point Kalman Filter (SPKF) (Contin.)
4. Cálculo de las aproximaciones de los momentos a priori de 1° y 2° orden de los estimados:
5. Calculo de los σ-puntos a priori de las variables observables y de las aproximaciones de sus momentos a priori de 1° y 2° orden:
6. Actualización del estimador y su covariancia a posteriori:
1 1 1 1 1 1 10 0
ˆ ˆ ˆ; ( ) ( )( )r r
i i i i i Tn n n n n n n n n
i i
P t− − − − − − −+ + + + + + +
= =
= = − −∑ ∑χχ ω χ γ χ χ χ χ
1 1 1 1 1 1 1 1 1 10 0
ˆˆ ˆ ˆ( ) ( )( ) ; ( ) ( )( )r r
i i i T i i i Ty k k k k k k y k k k k k k
i iP t y y y y P t y y− − − − − − − − − −
+ + + + + + + + + += =
= − − = − −∑ ∑χγ γ χ χ
1 1 1 1 10
ˆ[ ] ([ ]) [ ];r
i in n n n k k k
i
Y h y y− − − −+ + + + +
=
= Χ + Η = ∑ω
1
1
( )( ( ))
ˆ ˆ ˆ( )
( ) ( ) ( )( ( )) ( )
k y k y k
k k k k k
k k y k y k y k
K P t P t
K y y
P t P t P t P t P t
− − −
− −
− − − − −
=
= + −
= −
χ
χ χ χ χ
χ χ
19
20
Algoritmo del FKE I
• Inicialización: k=0
• Propagación de la estimación a priori del estado con mecanización INS:
• Predicción/estimación de las medidas externas
• Linealización del modelo alrededor de las estimaciones a priori
0
0
( ) 0ˆ ( ); ( ) ;
0 ( )x
o o op
P tE t P t
P tχχ χ⎡ ⎤
= = ⎢ ⎥⎣ ⎦
1
1
ˆ ˆ ˆ ˆ ˆ( , ); ( ) ; [ , )
ˆ ˆ ˆ( ) 0; ( ) ; [ , )ˆ ˆ( ) ( ( ), ( ),0)
k k k k
k k k k
i k
x f x m x t x t t t
p t p t p t t tm t M m t p t
+
+
= = ∈
= = ∈
=
11
ˆ ( )ˆ ˆk
kk
x tp
−− ++
⎡ ⎤χ = ⎢ ⎥⎣ ⎦
1 1 1ˆˆ ( ( ))k k ky h t− −+ + += χ
1 1 1 1 1 1
( ) ( ) ( ); ( ) ( ) ( );( ) ;
c c c
k k k k k k
F t B t t cov t t Q ty H t cov Rd+ + + + + +
= + == + =
δχ δχ ξ ξ δδ δχ η η
1ˆ ˆk k−+χ → χ
21
Algoritmo del FKE II
•Propagación de las covariancias a priori usando el modelo linealizado
•Cálculo de la Ganancia de Kalman y de la Covariancia a posteriori
•Forma alternativa por el Lema de Inversión de Matrices:
•Cálculo de la corrección de las estimaciones (variables de navegación y parámetros)
1 1 1 0 0; ( ); 0, ; ( )Tn n n n n k k kP P Q P P t n M P P t− −+ + + χ χ= Φ Φ + = = =
1
1 1
1 1
( , ) ( ( ) );
( , ) ( ) ( ) ( , ) ; ( ) ( )n
n
n n n c n st T T T
n n c c c n c n c c n st
t t exp F t T
Q t B t Q B t t d B t Q B t T+
+ +
+ +
Φ = Φ
= Φ τ Φ τ τ∫
( )k kP t Pχ χ→
( )
( )
1
1 1 1 1 1 1 1
1 1 1 1
1
1 1 1 1 1 1 1 1 1
( ) ( ) ( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
T Tk k k k k k k
k k k y k
T Tk k k k k k k k k
K P t H t H t P t H t R
P t P t K P t
P t P t H t H t P t H t R H t P t
−− −+ χ + + + χ + + +
− −χ + χ + + χ +
−− − − −χ + χ + + + χ + + + + χ +
= +
= −
= − +
1( )kx t +∆⎡ ⎤
1 1 1 1 1 1 1
11 1 11 1 1 1 1 1 1 1 1
: ( ) ( )
( ) ( ) ( ) ( ) ( ) ( )
− − − − − − −
−− − − −+ + + + + + + + +
+ = − + ⇒
⎡ ⎤= + ⇒ =⎣ ⎦T T
k k k k k k k k k
LMI A BD C A A B CA B D CA
P t P t H t R H t K P t H t Rχ χ χ
1( )t−+
1 1 1 1 1
1
ˆ( ) ( ( ))( )
i k k k k k
e k
p t K y y tp t
−+ + + + +
+
⎢ ⎥∆ = −⎢ ⎥⎢ ⎥∆⎣ ⎦
22
Ejemplo: INS + Baro-altímetro:
( );nz
nz z
z
Modelo
h f h
y h
γ
ξη
= +
== +
+∇∇
• Filtro “complementario”: – a) sigue variaciones rápidas de altura; – b) “ancla” la altura en bajas frecuencias.– c) estabiliza el canal vertical
modelocorreccionesmediciones∫ ∫
hhh
( )nz hγ
+nzf
0ˆ( )h t0
ˆ( )h t+
∫
-
k1(k)
( )ext kh t
z∇+
h
ε
- - -ˆ ( )z k∇
error(tk)
acelerom.
k2(k)
k3(k)
ˆ ( )z k∇
predicción(tk)
+
altímetro
23
Ejemplo de navegación integrada con ayuda de radar
Modelo del error para NAV=GEO (α=α’=0)
ψ
AU
AE
AN
Sistema de coordenadas de la antena
β
Rn n n n n
e
2
2tan sectan
( )
( 2 )
2( )
nib
n E N N EU
m n
E EU E
n nnnE
n n n n n n n
n n n n ne
n n n n
h V h VR h R h
V VV h
R h R hR h
φ φ δ δ δω
θ
δ φ δ δ
δ δ δ
δ δ δ
= × + − × −
ρ δ − δ ρ δ − δδ = − + δρ
+ +
Φ ΦΦδρ = δ − δ + δΦ
+ ++
δΦ = −δ
= × − + × − × +
+ × × + +
= + ×
ω ρ Ω
ρ E N U
V f ρ Ω V ρ V
Ω V f g
ρ ρ
θ
θθ θ
ni,n
n
Medición externa de posición:2 1/ 2 1 1( ) ] ; ;
A AA U Ei A A
H N
d dR d tg tgd d
− −⎛ ⎞ ⎛ ⎞⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟⎝ ⎠ ⎝ ⎠
∑ =[ = =Rango: Elevación: Azimut:β ψ
24
Navegación con ayuda de radar II:Ecuaciones de estado del error en tiempo continuo.
; ( ) ; ; / ( )
( ):
tan 10 0 0 0
tan
0 0 0 ( ) 2
En lat. y long. cosT n nE N U E N U E N E NU N
Tn n n n n ntE E N N U UE N U
V VE EU NR h R h R hn n m
V V V
g g g sesgos ruidos
h⎡ ⎤ =− = Φ⎣ ⎦
⎡ ⎤+ + + + +⎢ ⎥⎣ ⎦
ΦΩ + Ω −
+ + +−
Φ Φ
ξ
=
δ δθ = δθ
ε ε ε ∇ δ ∇ δ ∇ δ +
φ φ φ δ δ δ δθ δθ δ δ δθ δλ δθ
δ = δ +
−
µ
x
u Ω ρ
x F x u
F
0tan
2( )tan 1) 0 0 0 0
2( )2sec tantan0 0 0 0
( ) 2( )2tan sec
0 2( ) 0( ) ( )
(
VNU N
R hmVV VNE E
U UR h R h R hn m n R hn
VV V VNE E EN NR h R h R h R hn m n n R hn
V V V V V VN U N E E UV VU N U N N N U UR h R h R hn n n R hnf f
=+ Φ −
+
Φ+ − −
+ + + +
Φ ΦΦ+ − − −Ω+ + + + +
Φ Φ− − − − Ω + Ω −
+ + + +
Ω Ω
Ω Ω
Ω
µ µ
−
tan2 2( )
2 2 2tan ( ) sec tan0 2 0 )
( ) ( ) 2 2( ) ( )2
0 0 2 0( )( ) 2( )
10 0 0 0 0 0 0( ) 2( )
tan10 0 0 0 0( )
V VN ER hn
V V VV V VU N UE E EVU E U E E NR h R h R hn m n R h R hn m
V V VV VN N EE EVN E N E E UR h R h R h R hn m n m R hnVN
R hm R hmVE
R hn
f f
f f
Φ−
+
Φ Φ Φ− − − − Ω + −
+ + + + +
− + − − Ω −+ + + + +
− −+ +
−+
µ µ
µ µ
tan( ) ( ) 2( )
0 0 0 0 0 1 0 0 0
V VN ER h R hn m R hn
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥
ΦΦ⎢ ⎥− −⎢ ⎥+ + +⎢ ⎥⎢ ⎥⎣ ⎦
25
Navegación con ayuda de radar IIDiscretización de las ecuaciones de estado del error.
n 9
;
( )
(Sin sesgo de la IMU ni errores de gravedad: sólo rui[( ) ;( ) ; ; ; ]( ) ( );
( ) ( ) ( )do descorrelacionado)
Matriz de transición0 0 0 0 sk T
T T n TE N
) ) )(t,t t (t,t (t ,t (t ,
ht t
t H t t
φ
+= = ⇒Φ Φ Φ Φ
δ δ δ δ δ ∈δ = δ +ξδ = +ηδ
IF
x Vx F xy
θ θ
x
( )
( ) ( ) ( )
2 2 3max
11
10
/ 2 ( ); ( ( ))
, ;
;
1/
exp ( )( ) ( )
1 ( 1) ( 1)
, , ( 1) ,
Discretización d
ss
sk k
s s sk T k k k
s s s sSk k k k k k k kN
s s sk k k k k kj
)
) I o T t
Si ) ) )
t F t T(t ,t F t T F t T T
t t NT N (t j T ,t (t j T ,t jT (t jT ,t
t t t NT t t j T t jT
+
+−
+=
≈
+ + λ
=
Φ +
− = > ⇒Φ + + Φ + + + Φ +
Φ =Φ + = Φ + + +∏
F
( )( )
1 1 1
1 1 1
1
1
1 ;
;
( )
( ) ( )
( )
( ) ( )
el modelo del errorT
k k k k k
k k
T T
Tk k
dk
d
d
k k k
k dk dk k k k k k
k
k
k
t
t
k kt
t
k
k
k k k dk P E
E
E
(t ,t) t dt
H t t
Q (t ,t)Q t (t ,t)dt Q t t t
R t t
(t ,t ) (t ,t )+ + +
+ + +
+
+
+
ξ=
= =
≈
= + Φ ξ
+η
ξ ξ Φ Φ ≈ −
η η
δ δ
δ Φ δ =Φ δ +ξ δ δ∫
∫
y x
x x x x x
/ ( ) /s skT R t T=
26
Navegación con ayuda de radar IIIModelo de la medida externa de posición
: Posición de la antena conocida en terna "e" conocidaˆ, : Posición del vehículo y su estimación en terna "e"
= : Distancia vectorial real a la antena en terna "e"
eAe e
V V
e e e n eV A V A
P
P P
d P P P P
⇒
⎯⎯→= − − ←⎯⎯Ae
Ae
Cen
C
C
n n ne
[ , , ]ˆ ˆ ˆˆ ˆ ˆ ˆˆ= : Distancia vectorial estimada a la antena en terna "e" [ , , ]
ˆ ( )ˆ ˆ( ) ( )
ˆ
Tmed k k k
e e e n e TV A V A k k
e e e e A A e e nV V V n V
ne
med k k
R
d P P P P R
d d d P d P P P
β ψ η
β ψ
δ δ δ δ δ δ
δ δθ δ δθ
= +
= − − =
− = ⇒ = = =
⇒ −
−
en
A Ae e
e en n
y
C y
C C C
C S C C C S
y y , , , ,
ˆˆ ˆ ˆ( ) [ ( ) ] ( )
, , ˆ ˆ[ ( )
AEA e
k N k V kA A A A A AE N U E N UA
U
Ee n n n n n n n
V V V V V V p V N
n nk VA A A
E N U
dR Rd P
d d d d d dd
P P P P P P L Ph
R Pd d d
δβ ψ β ψ
δ δ η δ η
δ
δθδ δ δ δ δθ δ δθ
δ
β ψδ δθ
⎡ ⎤⎢ ⎥∂ ∂
= = + = +⎢ ⎥∂ ∂ ∂ ∂ ∂ ∂⎢ ⎥
⎣ ⎦⎡ ⎤⎢ ⎥= = + − + = ⎢ ⎥⎢ ⎥⎣ ⎦
∂= − +∂ ∂ ∂
Ae
e e e en n n n
A ee n
y C
C C C C S
y C C S [ ]
[ ]3 3 3 3 3 3 3 3
, ,
, ,0 0 ; ?
]
" "
pA A AE N U
p x x x
T
x pA A A
nV k E
E U
N k
N
R Ld d d
RH H H L Matriz de las medicionesd d
P h
d
δ η δθ δθ δβ
β
ηψ
ψ
∂=∂
+ +∂ ∂
∂= = = ←
∂ ∂ ∂
Ae
Ae
C
C
27
Navegación con ayuda de radar IVCálculo de la matriz de las mediciones
[ ]/ / / ; ( ) ( ) / ( ) ( ) / ( ) /
( ) ( ) ( ) ( ) ( ), , ( ) ( ) / ( ) ( ) / ( ) /
( ) /( ( )) ( ) /( ( )) 0
0
T TA A AE N UA A A A A A
E N U E N U
A A AE N U
n e eV V V
R d R d R d R S S R S C R C Rd d d d d d
C S C C SR S S R S C R C R
d d d S RC C RC
P P P
ββ ψ β ψ ψ
β ψ β ψ ββ ψ
β ψ β ψ ψψ β ψ β
δ
∂ ∂⎡ ⎤= = − −⎣ ⎦∂ ∂ ∂ ∂ ∂ ∂
⎡ ⎤∂ ⎢ ⎥= − −⎢ ⎥∂ ∂ ∂ ⎢ ⎥−⎣ ⎦
⎡ ⎤≈ −⎣ ⎦2 2
2 2
3 3
2
2 2
( ) 0 ( ) 1 ( )
0 ( ) 1 ( )
( )0ˆ( ) ( ) 0 ( ) (( , ) )
0 ( ) 1 )
,
(
,
T Tn
TnV
Nn
n n n E N U E nV V
AE N
n n
x A
R h o o
P h o o
i j k R h
P P R h R h TO hh ho o
RHd d d
ε ε
δ δ ε ε
δθ
δθ δ δθ δθ δθ δθ δθ δδε
β
εδ
ψ
ε
⎡ ⎤= + − +⎣ ⎦
⎡ ⎤= − +⎣ ⎦⎡ ⎤− +⎡ ⎤ ⎡ ⎤ ⎢ ⎥⎢ ⎥ ⎢ ⎥− + = + − − − + = + + ×⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥
∂
− + ⎣ ⎦ ⎣ ⎦⎣⇒
=∂ ∂
⎦
∂
S
( ) ( ) ( ) ( ) ( ) 0 ( ) 0ˆ( ) ( ) / ( ) ( ) / ( ) / ( ) 0 0
( ) /( ( )) ( ) /( ( )) 0 0 0 1
p
ne
p n nAU
L
C S C C S R hL S S R S C R C R R h
S RC C RC
β ψ β ψ ββ ψ β ψ ψψ β ψ β
− +⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥= − − +⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦
A Ae eC C C
VP
n vR h+
2( )oδ ≈ ε
Un
Nn
2 1/ 2 1 1( ) ] ; ;A A
A U Ei A A
H N
d dR d tg tgd d
β ψ− −⎛ ⎞ ⎛ ⎞⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟⎝ ⎠ ⎝ ⎠
∑=[ = =
28
Navegación con ayuda de radar VActualización del estado a posteriori (después de las medidas)
11 1/ 1/
1
1/ k / k
1/ 1 1/ 1/
1 1 1/1 1nˆ
(t ) (t )
( ) ˆ ˆˆ ˆˆ[( ) ;( ) ; ; ; ]δ φ δ δ δ δ
+
+ + + +
+ + + +
−+ + +
+
+
= +
= −
=
⎡ ⎤= +⎣ ⎦
− = V θ θ
Tk k k k dk
k k k k
Tk k k k k k k k
k k k
Tk
dk
k k k
k
k kT n T
E N
K P H H P H RK
P F P F Q
P P
y
H P
x yK h
Actualización de la covariancia, de la ganancia y del vector de estado
Corrección de la posición
( )
( ) ( )
ˆ ˆ ˆ ˆ ˆ( )ˆ ˆ
ˆ ˆ ˆ ˆexp( ( )) ( )
n n n n nE N U N
nc
n nc
tg
h h h
I
⎡ ⎤= = Φ⎣ ⎦
= +
= ≈ +n n ne e eC S C S C
δθ δθ δθ δθ δθ
δ
δθ δθ
Corrección de la actitud
Corrección de la velocidad
( ) ( )ˆ ˆ ˆ ˆexp( ( )) ( )n nc I= φ ≈ + φn n n
b b bC S C S C
( )ˆ ˆ ˆ ˆn n n n n ncV V V V V Vδ = − ⇒ = + δ
29
La alineación como caso particular de navegación integrada
INS
Kk
Mediciones: posición del vehículo fija y conocida
→Vmed = 0
( , )k kf x u
ˆ ( ) k kx k K error∆ =Corrección del estado
error δV=
Señales de losInstrumentosInerciales ˆ
INSV
ˆINSV
30
Navegación con ayuda de GPS
En caso que las mediciones externas sean Pe y Ve provistas por un GPS tenemos las relaciones:
vector error a estimar con el FK.TnV h⎡ ⎤δ φ δ δ δ⎣ ⎦x θ
0 ( ) 0 0ˆ 0 0 0 ;
0 0 0 1
EE
n Ne N e e
V p n n p V Vu
R hP L R h H V H
hh
δ δ δ δδ
δ
⎡ ⎤⎡ ⎤ ⎢ ⎥− +⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥= + = =⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎢ ⎥⎣ ⎦
δθδθδθδθ C x xδθ
Partiendo de la relación: e e n e e n e nV n V V n V n VV C V V C V C V= ⇒ = +δ δ δ
y de la definición: ( ) ( ) ( )e e n e n e n n e n nn n n V n V n VC C S C V C V C V= ⇒ = × = − ×θ θ θδ δ δ δ δ
( )3 3 3 1ˆ ˆ0 0
ne e e n
V n n Vn
VV C C S V H
h
× ×
⎡ ⎤⎢ ⎥⎢ ⎥⎡ ⎤= − =⎣ ⎦ ⎢ ⎥⎢ ⎥⎣ ⎦
xθ
φδ
δ δδδ
