Navegación Integrada:Ejemplos de Aplicación
Clase 6.1
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:β ψ
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
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥
ΦΦ⎢ ⎥− −⎢ ⎥+ + +⎢ ⎥⎢ ⎥⎣ ⎦
Navegación con ayuda de radar IIIDiscretizació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
( ) T
k jT
jk k k s
T T n TE N
k k E R T
ht t
t H t t E RH
t
φ
= =
δ δ δ δ δ ∈δ = δ +ξδ = +η η τ η λ = δ τ−λ
+η η η δδ δδ
x Vx F xyy x
θ θ
x
( ) ( )
1 1
1 0 1
1
110
1 1
;
( ) ; ( , ) ( ( ) ); ; 1
, , ;
de transición:
Propagación de la matriz de covariancia:
k
k
n c n s0 0 0 0 n n
n Mn S Sk k k k k
M
s nk k k k kn
n n
(t,t ) t (t,t ) (t ,t ) t t exp F t Tt t T t t t t t M T M
t t t M T t
P P
+ +
+ +
−
++=
−+ +
= +
Φ = Φ Φ = ⇒Φ =Φ= + ⇒ = = >
Φ =Φ + = Φ
=Φ
∏
F I
1 1 1 1 1
1
1 0 1
1 1
( ) ( ) ( ) ( )
; 0,1,... 1; ( ); ( )
( , ) ( ) ( ) ( , ) ; ( ) ( )
Cálculo de la ganacia y covariancia a posteriori:T
k x k k k x k
k
n
n
Tn n Mn k k k
t T T Tn c c c c n c c n sn nt
K P t H t H t P t H
Q n M P P t P P t
Q t B t Q B t t d B t Q B t T
− −+ + + + +
+
− − −χ χ+ +
+ +
=
Φ + = − = =
= Φ τ Φ τ τ∫
( )( )
1
1 1
1
1 1 1 1 1 1 1 1 1 1
( )
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
Tk k
T Tx k x k x k k k x k k k k x k
t R
P t P t P t H t H t P t H t R H t P t
−
+ +
−− − − −+ + + + + + + + + +
+
= − +
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
en
C
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
n ne U
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 tan( )
, , , ,ˆ
ˆˆ ˆ ˆ( ) [ ( ) ] ( )
, ,
nN
AEA e
med k k 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
k A A AE N U
dR Rd P
d d d d d dd
P P P P P P L Ph
Rd d d
θ
δβ ψ β ψ
δ δ η δ η
δ
δθδ δ δ δ δθ δ δθ
δ
β ψδ
δ
⎡ ⎤⎢ ⎥∂ ∂
− = = + = +⎢ ⎥∂ ∂ ∂ ∂ ∂ ∂⎢ ⎥
⎣ ⎦⎡ ⎤⎢ ⎥= = + − + = ⎢ ⎥⎢ ⎥⎣ ⎦
∂=∂ ∂ ∂
Φ
Ae
e e e en n n n
y y y C
C C C C S
y C [ ]
[ ]. 3 3 3 3 3 3 3 3
ˆ ˆ , ,
, ,0 0
[ ( ) ]
; ? " "
Tn n nV V k E NpA A A
E N U
rad x x x x pA A AE U
k
N
P P R Ld d d
RH H H L Matriz de las medicionesd d d
hδθ δ η δθ δθ δ ηβ ψ
β ψ
∂=∂ ∂ ∂
∂= = = ←
∂ ∂ ∂
− + + +Ae
A
An
e
ee C S C
C
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
β ψ− −⎛ ⎞ ⎛ ⎞⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟⎝ ⎠ ⎝ ⎠
∑=[ = =
Navegación con ayuda de radar VActualización del estado a posteriori (después de las medidas)
Calculo de la corrección del estimador a posteriori
11 1 1 1/n ˆ( )ˆ ˆˆ ˆˆ[( ) ;( ) ; ; ; ]δ φ δ δ δ δ ++ + + += = −V θ θ T
k k mk edk k kT n T
E Nx yK yh
Corrección de la posición
( ) ( )( )
ˆ ˆ ˆ ˆ ˆ( )ˆ ˆ ˆ ˆexp( ( )) ( )
ˆ ˆ
n n n n nE N U N
n nc
nc
tg
I
h h h
δθ δθ δθ δθ δθ
δθ δθ
δ
⎡ ⎤= = Φ⎣ ⎦
= ≈ +
= +
n n ne e eC S C S C
Corrección de la actitud
Corrección de la velocidad( ) ( )ˆ ˆ ˆ ˆexp( ( )) ( )n n
c I= φ ≈ + φn n nb b bC S C S C
( )ˆ ˆ ˆ ˆn n n n n ncV V V V V Vδ = − ⇒ = + δ
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
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
V p n n pu
R hP L R h H
hh
δ δδ
δ
⎡ ⎤⎡ ⎤ ⎢ ⎥− +⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥= + =⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎢ ⎥⎣ ⎦
δθδθδθδθ C 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θ
φδ
δ δδδ
SAR Aerotransportado (SARAT) de la CONAE
Beachcraft B200 utilizado para las pruebas del sistema de navegación.
Configuración Actual del Navegador del SARAT
Resultados de Navegación del SARAT: Reconstrucción Maniobra de Despegue
mts.
S.
O.
45 Km.
8 Km.
Resultados de Navegación del SARAT : Reconstrucción de la Trayectoria sobre el Plano Horizontal
Resultados de Navegación del SARAT :Reconstrucción de la Actitud
Orientación en función del tiempo. Altura en función del tiempo.
Desvios Estándar Teóricos del Error de Posición
Desvío estándar de la estimación en la posición horizontal
Desvío estándar de la estimación en la altura.
Desvíos Estándar Teóricos del Error de Orientación
La aceleración afecta la observabilidad del “rumbo”.Errores en pitch y roll cercanos a valores ideales. No así el rumbo.
Carreteo DespegueVel=0
Al inicio del vuelo. Durante el vuelo.
Períodos de vuelo casi uniforme.
Validación de Resultados
No se dispuso de sistema de navegación de precisión para validar resultados.Se comparan correcciones efectivas con sus intervalos de confianza teóricos.
Correcciones en latitud y altura e intervalo de confianza del 95%.
Correcciones en pitch y yaw e intervalo de confianza del 95%.
Estudio Comparativo de navegación inercial pura y navegación integrada
• Vehículo: Inyector Satelital Delta IV• Comparación navegación inercial pura con
– Instrumentos de calidad Navegación– Instrumentos de calidad Táctica.
• Comparación entre – Navegación INS-GPS con
• Medición de P y V• Instrumentos inerciales tácticos
– Instrumentos inerciales de calidad Navegacion
Navegación simulada del Delta-IV
•Giróscopos•Sesgo: [2.42e-8 -2.42e-8 -2.42e-8]rad/s•Factor de escala: [5e-6 5e-6 5e-6]•Falta de ortogonalidad:
•[oxy=oyx= 7.27e-5 ; oxz=ozx= 7.27e-5 ; oyz=ozy= 7.27e-5 ]rad•Desvío std: [2.42e-8 2.42e-8 2.42e-8] rad/s
•Acelerómetros•Bias: [1e-4 -1e-4 -1e-4 ] m/s2
•Factor de escala: [5.0e-4 5.0e-4 5.0e-4 ]•Falta de ortogonalidad:
•[oxy=oyx= 7.27e-5 ; oxz=ozx= 7.27e-5 ; oyz=ozy= 7.27e-5 ] rad•Desvío std: [1e-4 1e-4 1e-4] m/s2
•GPS: •Tiempos de adquisición c/1seg.•Ruidos blancos de varianza. Posición: 40m; Velocidad: 0.1m/s
Características de la Instrumentación Calidad Navegación
Valores de la inestabilidad en los parámetros:
Navegación simulada del Delta-IV
•Giróscopos•Bias: [52.36e-6 69.81e-6 131e-6] rad/s•Factor de escala: [1.1e-5 1.1e-5 1.1e-5]•Falta de ortogonalidad:
•[oxy=oyx= 9.77e-5; oxz=ozx= 3.5e-5; oyz=ozy= 1.06e-4]rad•Desvío std : [3.14e-5 3.14e-5 3.14e-5] rad/s
•Acelerómetros•Bias: [7.5e-4 -8.0e-4 -1.7e-4] m/s2
•Factor de escala: [3.0e-5 3.0e-5 3.0e-5]•Falta de ortogonalidad:
•[oxy=oyx= 2.1e-6 ; oxz=ozx= 3.0e-6 ; oyz=ozy= 3.5e-6] rad•Desvío std: [1.3e-4 1.3e-4 4.3e-4] m/s2
•GPS: •Tiempos de adquisición c/1seg.•Ruidos blancos de varianza. Posición: 40m; Velocidad: 0.1m/s
Características de la Instrumentación Calidad Táctica
Valores de la inestabilidad en los parámetros:
Simulación de la Navegación del Delta-IV
Trayectoria Nominal: Posición en Lat. Lon. y h respecto del WGS84
Simulación de la Navegación del Delta-IVTrayectoria Nominal: Velocidad respecto de la terna NAV=GEO-ENU
Simulación de la Navegación del Delta-IVTrayectoria Nominal: actitud terna del cuerpo respecto de la ECI en angs. de Euler
ECI=ECEF(t=0)
Simulación de la Navegación del Delta-IV
Trayectoria Nominal: Vel ang. ωib de la terna del cuerpo respecto de la ECI
Navegación Simulada del Delta-IV con Instrumentos Inerciales Tácticos: efectos de los errores instrumentales
Navegación Simulada del Delta-IV con Instrumentos InercialesNavegción: efectos de los errores instrumentales
Navegación Simulada del Delta-IV con Instrumentos InercialesTacticos: efectos de los errores instrumentales
Navegación Simulada del Delta-IV con Instrumentos InercialesNavegación: efectos de los errores instrumentales
Navegación Simulada del Delta-IV con InstrumentosInerciales Tácticos: efectos de los errores instrumentales
Navegación Simulada del Delta-IV con InstrumentosInerciales Navegación: efectos de los errores instrumentales
Simulación de la Navegación Integrada INS-GPS del Delta-IVcon instrumentos Tácticos
Simulación de la Navegación Integrada INS-GPS del Delta-IVcon instrumentos Tácticos
0.2
-0.2
0.3
-0.3
0.3
-0.3
Simulación de la Navegación Integrada INS-GPS del Delta-IVcon instrumentos Tácticos
Incidencia de los errores de los sensores inerciales: Calidadnavegación (navegación inercial pura)
Error en posición Error en velocidad Error en actitud
Bias de losgiróscopos
2.9 km 6.5 m/s 0.68°
Factor de escala del giróscopos
1.78 km 6.7 m/s 0.65 °
Ortogonalidad del giróscopos
2.77 km 5.74 m/s 0.2°
Bias de los acelerómetros 2.74 km 5.74 m/s 0.25°
Factor de escala del acelerómetros
3.5 km 7.8 m/s 0.25°
Ortogonalidad del acelerómetros
2.9 km 7.08 m/s 0.25°
Incidencia de los errores de los sensores inerciales: CalidadTáctica (navegación inercial pura)
Error en posición Error en velocidad Error en actitud Bias de los giróscopos
62.3 km 176.44 m/s 49°
Factor de escala del giróscopos
5.3 km 16.31 m/s 1.4°
Ortogonalidad del giróscopos
4.4 km 12.6 m/s 1.13°
Bias de los acelerómetros
4.7 km 15.24 m/s 1.44°
Factor de escala del acelerómetros
4.86 km 14.7 m/s 1.49°
Ortogonalidad del acelerómetros
4.98 km 15.06 m/s 1.41°
Incidencia de los errores de navegación para distintasconfiguraciones
Errorresmáximos
Error en posición
Error en velocidad
Error en actitud
INS de alta calidad
3.63 km 8 m/s 0.68°
INS de baja calidad
61.85 km 166.13 m/s 45.9°
IntegraciónGPS-INS baja
calidad
4.1 m 0.17 m/s 0.58°