1

Ecuaciones de Navegación

• Ecuaciones y “mecanización” en coordenadas inerciales• Mecanización para una referencia general.• Ecuaciones y “mecanización” en coordenadas ECEF• Ecuaciones en coordenadas NAV : ángulo de deriva• Rotación vehicular o “craft rate”• Filtro estabilizador del canal vertical• “Mecanización” en coordenadas NAV

2

Ecuaciones de Navegación en Coordenadas ECI (I)

Ecuación Fundamental de la Navegación Inercial

zi

xi

r

: ( ) ,

( ) ( ) ( ); ( ) :

( ) :

i i i i ig

i i b bb

bib

Newton t

t t t t

t

→ = + ⇒ =

= ←

←

ir g f V r r

f C f f

ω

Posición y velocidad :

Mediciones :fuerza específica Acelerómetros

velocidad angular Giróscopos

ri = Vector Posición en ECI.gg

i = Vector Aceleración Gravitacional en ECI.fi = Vector “fuerza específica” en ECI.

yi

3

Ecuaciones de Navegación en Coordenadas ECI (II)

( )

( ) ( ) ( ( )); (0) (0) (0) (0)

φ φ φ φφ φ φ φ

φ φ

: ( / ) ; : ( )

i i b i i e gb b ib b e g b

g g g g g

g ENU g g g g gb

g

t t t

C S S S S C C C S S S CC C S S C C S C S C S S

S S C C C

rumbo yaw azimuth geográfico cabeceo pitch

= =

⎡ ⎤θ ψ θ ψ + ψ θ ψ − ψ⎢ ⎥= θ ψ θ ψ − ψ θ ψ + ψ⎢ ⎥⎢ ⎥θ − θ − θ⎣ ⎦

ψ θ

C C S ω C C C C

C

: Actitud

( )

1 1

; φ :

0 ; lat. geodésica

( , , ) ( ) Expresado en componentes esféricas

; sin ( ); tan ( )

cos( ) 0 s

eg ENU

i i cg c g

c eg c

e ee e i

i c e e

ie

rolidoC S S C S

C SS S C C C

SHC

y xr y

t

− −

λ − Φ λ Φ λ⎡ ⎤⎢ ⎥= Φ Φ Φ ≡⎢ ⎥⎢ ⎥− λ − Φ λ Φ λ⎣ ⎦

=

Φ λ ≡

= Φ = λ =

Ω=

C

g C g

g r

r C r

C

Gravitación :

in( )0 1 0 ; 0

sin( ) 0 cos( )

c ci i e ic e c e c c

c c

t C S S C SC S

t t S S C C C

Ω λ − Φ λ Φ λ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥= = Φ Φ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− Ω Ω − λ − Φ λ Φ λ⎣ ⎦ ⎣ ⎦

C C C C

xe

tΩze

xi

yi =yeΩ

zi

EcuadorMeridianode Gr.

r

cΦ

λ

4

Mecanización de las Ecuaciones de Navegación en ECI

bibω

Giros.

∫ ∫iriViV

( , , ) ( , , )i i c eg c c g HSC c= Φ λ Ω Φ λg C g r

+

ifibC

bf+

0( )i tr0( )i tV

0 0 0 0( ) ( ) ( ) ( )i i e gb e g bt t t t=

C.I. :C C C C

i i ig= +V g f

( ) ( ) ( ( ))i i bb b ibt t t=C C S ω

eiC

etΩ

er

cos( ) 0 sin( )0 1 0 0

sin( ) 0 cos( )

C C Ci i ec e c

c c

c c

c c

t t C S S C SC S

t t S S C C C

= =

Ω Ω λ − Φ λ Φ λ⎡ ⎤⎡ ⎤⎢ ⎥⎢ ⎥ ⋅ Φ Φ⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥− Ω Ω − λ − Φ λ Φ λ⎣ ⎦ ⎣ ⎦

1

1

sin ( );

tan ( )

e

c e

ee

yr

xy

−

−

Φ =

λ =

( )

( )

0 ; longitud. geodésica

φ φ φ φ

φ φ φ φφ φ

eg ENU

g g g g g

g ENU g g g g gb

C S S C SC S

S S C C C

C S S S S C C C S S S C

C C S S C C S C S C S SS S C C C

λ − Φ λ Φ λ⎡ ⎤⎢ ⎥= Φ Φ Φ ≡⎢ ⎥⎢ ⎥− λ − Φ λ Φ λ⎣ ⎦⎡ ⎤θ ψ θ ψ + ψ θ ψ − ψ⎢ ⎥

= θ ψ θ ψ − ψ θ ψ + ψ⎢ ⎥⎢ ⎥θ − θ − θ⎢ ⎥⎣ ⎦

C

C

: ; ( ); : ( ), φ : g rumbo geográfico yaw cabeceo pitch rolidoψ θ

∫

Información de posición

Acelerómetros

gig

Información de actitud

5

Mecanización de las Ecuaciones de Navegación: Esquema General

( )b tf kf

kgg

+

+

,,

,

,

e

i i

c

e ik k

Φ

λ

er Vr V

C C

Modelo de Gravitación

kcC

cgg

( , , )c eg cΦ λg r

eΩ( )bib tω

Lat., long. altitud, posiciónactitud, Veloc. etc.

( )bik tω

+ ( )bbk tω

Integración de ecs. de navegación: Cálculos de velocidades (lineal/angular) posición, actitud y transformaciones.

Instrumentos (IMU)

Acelerómetros

Giros.

( )kb tC

Correcc. no inerc.

+

“k” = Nav, ECEF, ECI, etc.

−

Vernal, zi

M. de Greenwich

tΩ

xe

yi =yeΩ

ze

(1)e e i i i ei e= ⇒ =r C r r C r

derivando (1)

( ) + (2)

derivando (2) y usando .

( ) ( ) + ( ) ( )

[ (

i i e i e i e e i ee e e ie e

e eie

i i e e e i e e i e e i ee ie ie e ie e ie e

i i e e ee

const

⇒

= + =

= = ⇒

= + +

= × ×

r C r C r C S ω r C r

ω Ω

r C S ω S ω r C S ω r C S ω r C r

r C Ω Ω r ) 2( ) ]e e e ig+ × + = + iΩ r r g f

( ) 2( )e e e e e e e eg= + − × × − ×r f g Ω Ω r Ω rFinalmente…

Acel. de Coriolis

Ecuador

Ecuación Fundamental de la Navegación Inercial en Coordenadas ECEF

xi

Fuerza Centrífuga

re = Vector Posición del móvil en ECEF.

Respectivamente Vel. y Acel. del móvil respecto de la Tierra en ECEF.

Ωe = [0 Ωe 0]T= Vel. Ang. de la Tierra en ECEF.gg

e , fe = Gravitación y fuerza específica en ECEF.= Gravedad o gravitación aparente.

;e ee eV r V r

( )e e e e eg − × ×g g Ω Ω r

6

7

Mecanización de las Ecuaciones de Navegación en ECEF

2( )e e e e e= + − ×V f g Ω V

( , ) ( , , )

( , , )

e e c ec c g HSC c

e e e

e gg corr h

= Φ λ Φ λ −

− × ×

= λ Φ

g C g r

Ω Ω rC γ

( ) ( ) ( )e e b bb b ibt t= −C C S ω Ω

bibω

Giros.

Acelerómetrosbf ef

eΩ

eV

b b beb ib ie

b bib

= −

= −

ω ω ω

ω Ω

∫egb b e

e=Ω CΩ

bΩ

eΩ er

Calc. de Φc,Φ,λ,h

ebC

0 0. . ( ); ( )e et t≡CI r V

0 0 0( ) ( ) ( )e e gb g bt t t≡ =C.I. C C C

∫

Información de posición

Información de actitud

Ecuación Fundamental de la Navegación en terna NAV

Usamos… ;

("Ground Speed")

[ ( ) 2( ) ]

n n e e ee

n n e n ee e

i i e e e e e ee

= =

= =

= × × + × +

r C r V r

V C V C r

r C Ω Ω r Ω r r

8

( ) + = ( ) = ( )

[ ( ) 2( )]

[ ( ) 2( )]

Usamos......

derivando n

n n e e n e e e n e e e n e ee ne e n ne n en

i i e n e e e e e e ee n ne

i e n n n n n n n ne n ne

⇒

= → − +

= − × + × × + ×

= − × + × × + ×

V

V C S ω r C r r C V S ω r C V S ω r

r C C V ω r Ω Ω r Ω r

C C V ω V Ω Ω r Ω V

, para obtener....

[ ( ) )]

Finalmente:

n n n n nen ie en in

i i n n n n n n n nn in

+ = + =

= + × + × × + ×

ω ω ω Ω ω

r C V ω V Ω Ω r Ω V

(2 ) ( )

" " ( )

n n n n n n n n nen g

nen rotación vehicular craft rate

∴ = − + × − × × + +V Ω ω V Ω Ω r g f

ω

9

Rotación VehicularDefinición de rotación vehicular (“craft rate”): ρ ≡ ωen

“Rotación de la terna de navegación debida al desplazamiento del vehículo: Vn = 0 ⇒ ρ=0”

NE

m

VR h

ρ = −Φ = −+

ze

λ

ρe

Vn E, xg

N, ygU, zg

cos( )( )

EN

n

VR h

ρ = λ Φ =+

ze

Φλ

ρn

xe

ye

E, xg

N, ygU, zg

Ve

( ) cos( )nR h+ Φ

Rm +h = radio de curvatura meridiano

Φ

Suponemos α= 0 (ángulo de deriva nulo) es decir: “NAV” ≡ “g-ENU” ycalculamos ρg(Vg):

in( ) tan( )( )

EU

n

VR h

ρ = λ Φ = Φ+

ρu

ye

xe

gE N U g= ρ + ρ + ρgρ x zgy

Rn+h= radio“normal”

( )cos( )E

n

VR h

λ =+ Φ

Φ

VU= no produce rotación de la terna g-ENU.

10

Rotación VehicularEn general α≠ 0 (ángulo de ruta no-nulo): “NAV” ≠ “g-ENU”

Determinación de la rotación vehicular: ρn(Vg) y ρn(Vn)

gE N U g= ρ +ρ +ρ ⇒gρ x zgy

1

1

0

0M

M

E E E

N N N

n

Rm

R

h V VV V

h

+

+

⎡ ⎤−⎢ ⎥ρ⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥= =⎢ ⎥ ⎢ ⎥ ⎢ ⎥ρ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎢ ⎥⎣ ⎦

;ngρ C ρ

xn g

y

z

⎡ ⎤ρ⎢ ⎥

= ρ =⎢ ⎥⎢ ⎥ρ⎣ ⎦

2

2 2

2

x x yn n

y x yn m n

C

R h R h R h R hm m

R h R h R h R hm

C S C S SV V

S C C S C SV V

++ + + +

+ + + +

⎛ ⎞⎛ ⎞α α α α α αρ = − − ⎜ ⎟⎜ ⎟ ⎜ ⎟⎝ ⎠ ⎝ ⎠

⎛ ⎞ ⎛ ⎞α α α α α αρ = + + −⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎝ ⎠⎝ ⎠

, ( )nx y →ρ Vn

sin cos

cos sin

x E Nn

y E Nn

R h R hm

R h R hm

V V

V V

+ +

+ +

α α

α α

ρ = −

ρ = +

, ( )nx y →ρ Vg

M

M

x E

y N

x

y

VC SVS C

VC S C SVS C S C

ρ⎡ ⎤ α α ⎡ ⎤⎡ ⎤= =⎢ ⎥ ⎢ ⎥⎢ ⎥ρ − α α⎣ ⎦ ⎣ ⎦⎣ ⎦

⎡ ⎤α α α − α⎡ ⎤ ⎡ ⎤= ⎢ ⎥⎢ ⎥ ⎢ ⎥− α α α α⎣ ⎦ ⎣ ⎦ ⎣ ⎦

ρU= ρz , depende de la selección de α(t).

xn

E

N

α

//nU z≡ αyn

Si varía en coordenadas de la Tierra 0 0, 0,y, posiblemente también 0

P P⇒ ≠ ⇒ λ ≠ Φ ≠⇒ α ≠

,

, , ,

sin( )

sin( ) tan( )( )

n

U n

n n n n

E

n

VR h

= −Φ + λ + α ⇒

ρ = =

= −Φ + λ + α =

= λ Φ + α

λ Φ = Φ+

g

g

ρ x z

ρ z

z x z z z

e

e

y

y

NOTA: fuera del Ecuador, 0, genera rotación "U" de la terna de navegación,a menos que: sin( ) 0;U t

λ ≠

α = −λ Φ ⇒ ρ = ∀

ze λ

ρE

ρN

E, xg

N, yg

U, zgye

xeΦ

Φ

h

Φλ

P ρU

λ

Rotación Vehicular

Más sobre ρU cuando α≠ 0 (ángulo de ruta no-nulo): “NAV” ≠ “g-ENU”

11

12

Rotación VehicularFórmulas de la rotación vehicular.

Rot. vehic. en GEO. Rot. vehic. en NAV.

cos

tansin

NE

EN

n

EU

n

R hm

R h

R h

V

V

V

=+

+

+

ρ = − −Φ

ρ = = λ Φ

Φρ = = λ Φ s in ; co n a rb itra rio

yxx

y

yxy

z

x

VVT R

VVR T

ρ = −

ρ = −

ρ = λ Φ + α α

Definiciones:

( ) ( )

( ) ( )

( )

2 2

2

2 21 (1 3 (1,2) (2,2 )

2 2 21 (1 3 (2,2) (1,2 )

2(2,2) (1,2 )

x

y

1 1R

1 1R

1T

h n nf e eaR h R hm

C h n nf e eaR h R hm

f n ne eaR h R hm

n

n

n

S Ca

Sa

C S C S

− − − −+ +

+ − − − −+ +

−+ +

⎛ ⎞α α ⎛ ⎞+ ⎜ ⎟⎜ ⎟ ⎝ ⎠⎝ ⎠⎛ ⎞α α ⎛ ⎞⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎝ ⎠⎝ ⎠

⎛ ⎞α α α α−⎜ ⎟

⎝ ⎠

C C

C C

C C

2

2 2 3 / 2

2 2 3 / 2

(1 )( )(1 sin ( ))

( )(1 sin ( ))

m

n

aR S

aR S

− ε− ε Φ

− ε Φ

13

Rotación Vehicular

Diversas elecciones de α(t) para coordenadas “NAV”

;

; cos sin ;

cos ( sin )

0

(cos sin )

tanEn los polos: , cuando ,!2

.

n n g

n

N En U g

m n

EU z z

n

R

R

V VR h h

Vh

+

+

= −Φ + λ + α = Φ + Φ =

= −Φ + λ Φ + α + λ Φ

⇒ α = α = ⇒

= −Φ + λ Φ + Φ + α = − + + ρ+

Φ πρ = ρ = ρ → ∞ Φ →

g

g

g g

ρ x z z z z

ρ x z

1. - NAV = GEO

ρ x z z x z

2 -

e e g g

g

g g g

y y y

y

y y

" " sin 0 sin

cos 0 ; 0 . . : rotación vertical nula aun si !2

En los polos: ( ) ; ( ) 02 2

( ) depende del camino recorrido sobre la Tierra! Ej.

n U

U

i e

t

α + λ Φ = ⇒ α = −λ Φ

π= −Φ + λ Φ + ⇒ ρ = Φ =

π πΦ = ± = −Φ ρ Φ = ± =

α α

g

g

azimut libre :

ρ x z

ρ x

gy

,

. Sobre los meridianos

. 0 ( )sin : rotación inercial vertical nula !

cos sin

sin (Compensa la rotación de la Tierra);

En los polos: (

in z z z e

e n

U e

cte=

α ω = ρ + Ω = ⇒ α = − Ω + λ Φ

= −Φ + λ Φ −Ω Φ

ρ = −Ω Φ

Φ

g

2 - de Foucault :

ρ x z

ρ

gy

) sin ; ( ) .2 2e n U eπ π= ± = −Φ Ω Φ ρ Φ = ± = Ωgx z∓ ∓

14

Rotación Vehicularcos ( sin ) n= −Φ + λ Φ + α + λ Φgρ x zgy

Mecanización Angulo de rutaRotación Vehicular Vertical

Rotación Inercial Vertical

Rot. Inercial Vertical en los polos

Apuntamiento al Norte

Azimut Libre

Foucault

Unipolar Hemisferio Norte

Unipolar Hemisferio Sur

0 α = α =

sinα = −λ Φ

( )sineα = − Ω + λ Φ

α = λ

α = −λ

sinλ Φ

0=

0=

( )sineΩ + λ Φ

( )2πΦ = ±

sine−Ω Φ

sineΩ Φ

(sin 1)λ Φ −

(sin 1)λ Φ +(sin 1)

sine

λ Φ + +Ω Φ

(sin 1)sine

λ Φ − +Ω Φ

eΩ + λ → ∞

0=

eΩ

en 2e

Polo Norte

πΩ Φ = +

en 2e

Polo Sur

πΩ Φ = −

0 α ≠

0 α ≠

0 α ≠

0 α ≠

15

Ecuaciones de Cinemáticas en Coordenadas NAV (I)

( )

( 2 )

(

( ), ( ) ( ) ( ) ( );

( 2 ) : (0) (0) (0)

" " n n nz

x y

b b n n bib b

Tgn n n n n n n n ng x y z

n n nz z z

V

t t t t t

V V V

Canal vertical h V g f

→

⎡ ⎤⎣ ⎦

⎡ ⎤− + ×⎣ ⎦

= ρ

=

= − + × + + ≡ = ⇒ =

→ = = + +ρ Ω V

f ω f C f

V ρ Ω V g f CI V C V V

Fuerza especifica y velocidad angular :

Velocidad :

Posición :

,

1 1

2

2

2 ) ( 2 ) ; (0)

:

( ) ( ) ( ) ( ) ( ) ( ):

(3,1)sin (3,2); tan ,(3,3)

tan( )tan( )

n ny y x x

n nz z

n n n n n ne e n e e e

nn ee n

e

c

V g f h

Posición sobre el Elipsoide

Actualizacion de t t t tCoordenadas Curvilíneas

ba

− −

+ Ω − ρ + Ω

→

+ + ≡

= = − ⇒

Φ = λ =

ΦΦ = α =

CI

C C S ω C S ρ C

CCC

1

2

(1,2)tan , ( , , ) , , ,(2,2)

:

( )cos( )cos( )( )sin( )cos( )( (1 ) )sin( )

ne

x y nne

en

e en

en

h T R R R

Coordenadas Cartesianas

x R hy R hz R h

−

⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦⎣ ⎦

Φ λ →

+ λ Φ= = + λ Φ

− ε + Φ

CC

r

16

Ecuaciones Cinemáticas en Coordenadas NAV (II)

( )

; ; sin ; arbitrario

( ) ( ) ( ( )); (0) (0) (0)

( ) ( ) ( ) ( ) ( ) ( )

y yx xx y z

y x

gn n b n nb b nb b g b

n

n n b n n e nnb ib in ib en b b ib e b

ng ENU

V VV VT R R T

t t t

t t t

→ ρ = − ρ = − ρ = λ Φ + α α

= ≡ =

= − = − + ⇒ = − +

=

C C S ω CI C C C

Ωω ω ω ω ω Ω C C S ω S ρ C Ω C

C

"Craft Rate" :

Actitud : ρ

( )

00 ;

0 0 1

φ φ φ φ

φ φ φ φφ φ

: ; ( ); : ( ), φ :

( , )

g g g g g

g ENU g g g g gb

g

n e ce c c g H

C SS C

C S S S S C C C S S S C

C C S S C C S C S C S SS S C C C

rumbo geográfico yaw cabeceo pitch rolido

α − α⎡ ⎤⎢ ⎥α α⎢ ⎥⎢ ⎥⎣ ⎦⎡ ⎤θ ψ θ ψ + ψ θ ψ − ψ⎢ ⎥

= θ ψ θ ψ − ψ θ ψ + ψ⎢ ⎥⎢ ⎥θ − θ − θ⎢ ⎥⎣ ⎦

ψ θ

= Φ λng C C g

C

Gravedad :

[ ]

( )2

2

( , , )

( , , ); 0 0 :

( ) 1 2 1 2 sin ( ) 3

e n n n eSC c e

Tn g gg corr

s

h

h hf m fa a

Φ λ − × ×

= λ Φ = −γ

⎛ ⎞⎛ ⎞γ = γ Φ − + + − Φ +⎜ ⎟⎜ ⎟⎜ ⎟⎝ ⎠⎝ ⎠

r Ω Ω C r

C γ γ

17

Dinámica del “canal vertical” en lazo abierto I

GravedadEcuación cinemática

(2 )

( , )

n n n nz z

n nz z

h V

h f

⎡ ⎤= = − + × +⎣ ⎦+ γ Φ +

Ω ρ V ( )2

2

2

( , ) ( ) 1 2 1 2 sin ( ) 3

( , ) ( ) 1 2 ( ) ( )

nz s

nz s

h hh f m fa a

hh A O ha

⎛ ⎞⎛ ⎞γ Φ = −γ Φ − + + − Φ +⎜ ⎟⎜ ⎟⎜ ⎟⎝ ⎠⎝ ⎠⎡ ⎤γ Φ = −γ Φ − Φ +⎢ ⎥⎣ ⎦

Mecanización

∫ ∫hhh

2( , ) ( ) 1 2 ( ) ( )nz s

hh A O ha

⎡ ⎤γ Φ = −γ Φ − Φ +⎢ ⎥⎣ ⎦

nzf

-0

ˆ( )h t0ˆ( )h t(2 )n n n

z⎡ ⎤+ ×⎣ ⎦Ω ρ V

+

z∇

+ +

18

Inestabilidad del “canal vertical” en lazo abierto II

Propagación de los errores en lazo abierto0 0

0 0 0 0 0 0

1

ˆˆ. . : ( ) ( ) ( ); ( ) ( ) ( );ˆ (" ")

ˆ ˆ( ) ( ) ( ) ( , ) ( , ) ( ) 2 ( ) ( ) ( )

. :

zn n

z z z

n nz z z s z

Sean C I h h t h t h t V h t h t h t

f f error de acelerómetro bias

h t h t h t h h h t A a h t

Ecuación carcterística

−

δ = δ − δ =δ −

∇ − ≡

⇒ δ − = γ Φ − γ Φ +∇ ⇒ δ ≈ γ Φ Φ δ +∇

⇒ λ2 1

11,2

1 1 1

00

2 ( ) ( ) 0,

2 ( ) ( ) !!!!

2( ) ( ) sec 0.0012sec ;84x60

( ) cosh( 2 ) 1 cosh( 2 ) sinh( 2 )2 2

( )2

!!!s

s

s s

z zs s s

s s

zz

A a

A a

A a

Vh t t h t t

d hV tdt

−

−

− − −

− γ Φ Φ =

⇒ λ = ± γ Φ Φ

π≡ ω γ Φ Φ ≈ ≈

∇ δ⎡ ⎤δ = ω − + δ ω + ω⎣ ⎦ω ω

∇δδ = =

⇒ solución inestable

Frecuencia de Schuller

0 0

00

sinh( 2 ) 2 sinh( 2 ) cosh( 2 )

( ) 0; 0; 1 ( 15min) 1300 . !!

s s s z ss

mz z s

t h t V t

h t V h t m de desviación

ω + δ ω ω + δ ωω

δ = ∇ = δ → δ = ≈NOTA : ∼

19

Filtro estabilizador del “canal vertical”

[ ][ ]

1 2 2

1 2

1 2

; (2 ) ( , )

;

( ) 1 0(0) (0); (0) (0); 0

n n n n nz zz

TTT

z z

z z z h f

z z h h

h tz h z V

⎡ ⎤= = − + × + γ Φ +⎣ ⎦

⎡ ⎤= = ⎣ ⎦=

= = ∇ =

Ω ρ V

z

z(2 )n n n

z⎡ ⎤+ ×⎣ ⎦Ω ρ V

∫ ∫h1ζ

2( , ) ( ) 1 2 ( ) ( )nz s

hh A O ha

⎡ ⎤γ Φ = −γ Φ − Φ +⎢ ⎥⎣ ⎦

+nzf

-

0ˆ( )h t0

ˆ( )h t+

3k ∫ -

k2 k1

medh

z∇

+

h

hε

- - -3

ˆ( )zζ ∇

1ζ2ζ2ζ

Ecuación de estado cinemática “ideal”.

+

Ecuación de estado del “filtro” vertical realimentado.

[ ][ ][ ]

1 2 1 1

2 1 2 1 3

3 3 1

1 2 3 1 2 3

1 1

1 1 2 2 3

( );ˆ(2 ) ( , ) ( )

( )ˆ ˆ ˆ; (0) (0); (0) (0); (0)

ˆ( ) 1 0 0 ;

; 0

(2 )

medn n n n n

z z medz

medTT

z z

medTT

z z

n n nz

k h

f k h

k h

h V

h t h h z

z z

de

ζ = ζ − ζ −

⎡ ⎤ζ = − + × + γ ζ Φ + − ζ − −ζ⎣ ⎦

ζ = ζ −

= ζ ζ ζ ζ = ζ = ζ =∇

= = ζ = +ε = + ε

ζ − ζ − ζ −∇ ∇ ≠

⎡ ⎤+ × →⎣ ⎦

Ω ρ V

ζ

ζ

e

Ω ρ V " "l canal horizontal supuesto sin error

Ecuación del error en lazo cerrado.

1 2 1 1 2 1 1

2 1 1 3 2 1

22 1 2 1 3

3 3 1 1 3 1

1 12

2 2

3 3

( ) ( );ˆ( , ) ( , ) ( )

2 ( )( ) ( )

1

2 0 10 0

z

medn n n nz z z z

s

s

e e k h e k e

e z f f k e

e e k e ee k z k e

k 0 kk k

k k

∇

= − ζ − = − −ε

= γ ζ Φ − γ Φ + − −ζ − −ε

ω − −ε −

= ζ − −ε = −ε

−⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥= − + ω − + ε⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥−⎣ ⎦⎣ ⎦

e e

20

Diseño del filtro/lazo estabilizador del canal vertical

[ ]

1 1 2 2 3

1 12

2 2

3 3

3 2 21 2 3

12 0 1

0 0

. : ( ) ( 2 ) 00 . 0 0.

TTz

s h

s

i h

z z

k 0 kk k

k k

v propios raicesde p k k kRE lazo estable t si

ζ − ζ − ζ −∇

−⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥= − + ω − + ε ⇒⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦

λ = λ +λ +λ − ω + =

λ < ⇒ <∞ ∀ → ε =

e

e e

e e

(2 )n n n

z⎡ ⎤+ ×⎣ ⎦Ω ρ V

∫ ∫1

ˆ ( )h ζ1ζ

2( , ) ( ) 1 2 ( ) ( )nz s

hh A O ha

⎡ ⎤γ Φ = −γ Φ − Φ +⎢ ⎥⎣ ⎦

+nzf

-

0ˆ( )h t0

ˆ( )h t+

3k ∫ -

k2 k1

medh

z∇

+

h

hε

- - -3

ˆ ( )z∇ ζ

h

2ζ

Dinámica del error de estimación

+

Diseño:• Dinámica rápida: sigue los cambios rápidos en aceleración ; aumenta

efecto del ruido de la medida externa.• Dinámica lenta: atenúa bien ruidos de medida ; acentúa efectos errores

en acelerómetro y condiciones iniciales.• Constantes de tiempo típicas: 40 sec a 400 sec. x

x

x 45o

1/T

01 2,3

3 2 3 2 22 3 1 2 3

2 2 1 4 2 6 32 31 2 3 1 2 3

1 1 1 1 1: ; 1,2,3, . cos(45 ) ,2

3 4 2( ) ( 2 )

3 4 2; 2 ; ; 100sec 3.0 10 sec ; 4.0 10 sec 2.0 10 sec

i

s

s

Elejimos RE i amortig jT T T T

p k k kT T Tk k k T k x k x k xT T T

− − − − − −

λ = − = ξ = = ⇒ λ = − λ = − ±

λ = λ + λ + λ + = λ +λ +λ − ω + ⇒

= = + ω = = → = = =

21

Resumen de las ecuaciones de navegación en coordenadas NAV

1

3 2

ˆ ˆ( ), ( ) .

ˆ ˆ ˆˆˆ ˆ( 2 ) :

ˆ ˆ ˆˆ ˆ ˆ ˆˆ ˆ ˆ" " ( 2 ) ( 2 )

ˆ ˆˆ ( )

ˆ ˆ( ) ( )

b bib

n n n n n n

n n n n nz x y y y x x z z

nz med

med med

t t

u

Canal vertical h V V V g f u

h V k h h

u k h h dt k h h

Pos

= − + × + + +

→ = = ρ + Ω − ρ + Ω + + +

= − −

= − − − −∫

n

f ω

V ρ Ω V g f z

Mediciones :

Velocidad :

Posición :

ˆ ˆ ˆ ˆˆ ˆ( ) ( ) ( ) ( , , )

ˆ ˆˆ ˆ ˆ ˆˆˆ ˆ; ; sin ; arbitrario

ˆ ˆ ˆˆ ˆ( ) ( )

ˆ ˆˆ

n n ne e

y yx xx y z

y x

n n b n nb b ib in b

ne

ición angular sobre el Elipsoide t t

V VV VT R R T

→ = − → λ Φ α

→ ρ = − ρ = − ρ = λ Φ + α α

= −

=n

C S ρ C

C C S ω S ω C

g C C

"Craft Rate" :

Actitud :

Gravedad :

[ ]

ˆ ˆ ˆ ˆ ˆˆ ˆˆ ˆ( , ) ( , , )ˆˆ ˆ ˆ ˆ ˆ( , , ); 0 0 :

e c e n n n ec c g HSC c e

Tn g gg corr h

Φ λ Φ λ − × ×

= λ Φ = −γ

g r Ω Ω C r

C γ γ

22

Mecanización Canales Horizontal y Vertical en NAV

acel

giros

( ) ( )C C S ω S ω Cn n b n nb b ib in b= −

nbC

∫

f

bx

b by

bz

f

f

f

⎡ ⎤⎢ ⎥

= ⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

f

nx

n ny

nz

f

f

f

⎡ ⎤⎢ ⎥

= ⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

, , ,b b b bib ib x ib y ib z

T⎡ ⎤= ω ω ω⎣ ⎦ω

Mecanización del canal Vertical

nzV n

xV nyV

nzf

nyfn

xf

medh

, ,Φ λ α

, , ,Tn n n n

in in x in y in z⎡ ⎤= ω ω ω⎣ ⎦ω

ninω

Instrumentos

Gimbal Analítico

h

Mecanización del canal horizontal

23

Mecanización de las ecuaciones de navegación en Coordenadas NAV

( 2 )n n n n n n nu= − + × + + +V ρ Ω V g f z

( , ) ( , , )

( , , )

n e c ee c c g HSC c

n n n

n e ge g corr h

= Φ λ Φ λ

− × ×

= λ Φ

ng C C g r

Ω Ω rC C γ

( ) ( )n n b n n nb b ib b= − +C C S ω S ρ Ω C

bibω

Giros.

Aceleróm.bf nfn

bC

nΩ

nV

ρn

( )nb ib in ib en= − = − +ω ω ω ω ω Ωρ

u

ngρn

;

s in ; se lc . a rb itr.

y yx xx y

y x

z

V VV VT R R T

ρ = − ρ = −

ρ = λ Φ + α α

( )e e nn n=C C S ρ

1

1

1

2

2

sin (3,2)(3,1)

tan ,(3,3)(1,2)

tan ,(2,2)tan( )tan( )

nenenenene

cb

a

−

−

−

Φ =

λ =

α =

ΦΦ =

CCCCC

n n ee=Ω C Ω

neC

Altím.medh

h

h1 3 1 2

1

;nz

u k h u u k h

h V k h

= = +

= −

nzV

hInformación de

posición

Información de actitud