Navegación Integrada: Ejemplos de...

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°