ecuaciones de navegaciónlaboratorios.fi.uba.ar/lscm/espana/apuntes/clase3_1_ecs...mecanización de...
TRANSCRIPT
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