Inertial Navigation Mechanization and Error Equations

Μέγεθος: px

Εμφάνιση ξεκινά από τη σελίδα:

Download "Inertial Navigation Mechanization and Error Equations"

ÏἈχαϊκός Λύτρας
6 χρόνια πριν
Προβολές:

1 Iertial Navigatio Mechaizatio ad Error Equatios 1 Navigatio i Earth-cetered coordiates Coordiate systems: i iertial coordiate system; ECI. e earth fixed coordiate system; ECEF. avigatio coordiate system; local level, NED. Fudametal equatio of iertial avigatio: r i = f i + G i Navigatio i a coordiate system fixed to the Earth is more practical: First derivative: Secod derivative: r i = C i e re ṙ i = Ċi e re + C i eṙe = C i eω e i/er e + C i eṙe r i = Ċi eω e i/er e + C i e Ω e i/er e + C i eω e i/eṙe + Ċi eṙe + C i e re = CeΩ i e i/eω e i/er e + Ce i Ω e i/er e + CeΩ i e i/eṙe + CeΩ i e i/eṙe + C e re i = Ce (Ω i ei/eωei/ere + Ω ei/er ) e + 2Ω ei/eṙe + r e Combiig with the fudametal equatio: f e + G e = ω e i/e ωe i/e }{{ re + ω e i/e } re + 2ω e i/e }{{} ṙe + r e }{{} Cetripetal Euler Coriolis 1

2 2 Navigatio i local-level coordiates It is eve more practical to calculate velocities i local level coordiates (velocity east, velocity orth), ad calculate positio i geodetic coordiates (latitude, logitude). 2.1 Velocity equatio By defiitio: First derivative: v C e ṙe v = Ċ e ṙe + C e re = C e Ω e /eṙe + C e re Solve for r e : r e = C e v Ω e /eṙe Now, repeatig the Earth cetered avigatio equatio from the previous sectio with the Euler term eglected: f e + G e = Ω e i/e Ωe i/e }{{ re + 2Ω e } i/eṙe + r e }{{}}{{} First term Secod term Third term We will covert the above equatio to avigatio coordiates term by term. First term: Secod term: Third term: ( ( ) Ω e i/eω e i/er e = C e C e Ω e i/ec) e C e Ω e i/ec e C e r e = C e Ω i/eω i/er 2Ω e i/eṙe = 2Ω e i/e Ce ( v ) = 2C e C e Ω e i/ec e v = 2C e Ω i/e v r e = C e v Ω e /eṙe ( ) = C e v C e C e Ω e /e Ce C e ṙ e = C e v C e Ω /e v 2

3 ecombiig: f e + G e = Ω e i/e Ωe i/e re + 2Ω e i/eṙe + r e = CΩ e i/eω i/er + 2CΩ e i/ev + C e v CΩ e /ev ( = C e Ω i/e Ω i/e r + 2Ω i/e v + v Ω /e ( v) = C e Ω i/e Ω i/e r + ( 2Ω i/e ) Ω /e v + v ) ( = C e Ω i/e Ω i/e r + ( 2Ω i/e + ) Ω e/ v + v ) ( = C e Ω i/e Ω i/e r + ( Ω i/e + ) Ω i/ v + v ) Ce (f e + G e ) = Ω i/e Ω i/e r + ( Ω i/e + ) Ω i/ v + v f + G = Ω i/eω i/er + ( ) Ω i/e + Ω i/ v + v ecall that g = G ω i/e (ω i/e r): f + g + Ω i/e Ω i/e r = Ω i/e Ω i/e r + ( Ω i/e + ) Ω i/ v + v f + g = ( Ω i/e + ) Ω i/ v + v v = ( ) ω i/e + ω i/ v + f + g The above equatio is itegrated i real time to calculate velocity i the avigatio coordiates. 2.2 Implemetatio Details The local Earth rate vector is give by ω i/e cos φ ω i/e =. ω i/e si φ The agular rate vector for the chage i the local level coordiate frame with respect to the Earth frame is give by λ cos φ ω e/ = φ λ si φ. So the agular rate vector for the Coriolis correctio is give by (2ω i/e + λ) cos φ ω i/e + ω i/ = 2ω i/e + ω e/ = φ (2ω i/e + λ) si φ. 3

4 Writig out the compoets of the matrices i the velocity equatio: v N (2ω i/e + λ) si φ φ v E = (2ω i/e + λ) si φ (2ω i/e + λ) v N cos φ v E v D φ (2ω i/e + λ) cos φ v D f N + f E + f D g The above equatio is itegrated i real time to obtai the velocity: v N (t) = v N (t ) + v E (t) = v E (t ) + v D (t) = v D (t ) + t ( ((2ω i/e + λ) si φ)v E + φv D + f N ) dτ t (((2ω i/e + λ) si φ)v N + ((2ω i/e + λ) cos φ)v D + f E ) dτ t ( φv N ((2ω i/e + λ) cos φ)v E + f D + g) dτ Note that the above implemetatio is ustable; a practical iertial avigatio system requires a exteral referece. Oe method for calculatig the latitude ad logitude rates is give by λ = φ = v N, v E cos φ. The whole values of latitude ad logitude are calculated by itegratig the rates. 3 Navigatio ψ-agle error equatios 3.1 Velocity error equatio Defie three coordiate systems omially aliged with avigatio or local level coordiates: t true coordiate frame; the local level coordiate frame at our true positio. 4

5 f N + v N (t ) v N φ φ(t ) φ f E + v E (t ) v E 1/ λ λ(t ) λ f D + + v D (t ) 1/( cos φ) h(t ) v D h g Coriolis 1 φ λ φ Figure 1: Iertial avigatio system velocity ad positio processig. c computed coordiate frame; the local level coordiate frame at the positio we have computed. The misaligmet agles betwee true ad computed coordiates are deoted by δθ. p platform coordiate frame. I a gimbaled system, the accelerometers are attached to a stable platform whose attitude is cotrolled by the gryos. The platform coordiate system is defied by the accelerometer sesitive axes. The misaligmet agles betwee computed ad platform coordiates are deoted by ψ. The system total attitude error, φ, is the misaligmet betwee the true ad platform coordiates ad is give by φ = ψ + δθ. The ψ-agle error equatios use the computed coordiate frame for error aalysis. The velocity equatio i the computed coordiate frame, but usig the true values of velocity, specific force, ad gravity: v c = ( ω c i/e + ) ωc i/c v c + f c + g c 5

6 The velocity equatio agai, but usig the values of velocity, specific force, ad gravity that are estimated i real time by the avigatio computer: ˆv c = ( ω c i/e + ωc i/c) ˆv c + ˆf c + ĝ c Each of the estimated values i the equatio above is the sum of the true value ad a error: ˆv c = v c + δv c The specific force vector is measured i the platform frame, but for the real time calculatios, it is assumed to be i the computed frame: ˆf c = f p + δf c Similarly, the gravity vector is sesed i the true frame, but for the real time calculatios, it is assumed to be i the computed frame: ĝ c = g t + δg c δh Substitutig ito the estimated velocity equatio: v c + δ v c = ( ω c i/e + ωc i/c) (v c + δv c ) + f p + δf c + g t + δg c δh Ad subtractig off the true values: δ v c = ( ω c i/e + ωc i/c) δv c + f p f c + δf c + g t g c + δg c δh f p = C p c f c = (I (ψ )) f c = f c ψ f c g t = Ccg t c = (I + (δθ )) g c = g c + δθ g c The velocity error equatio: δ v c = ( ω c i/e + ) ωc i/c δv c ψ f c + δf c + δθ g c + δg c 6

7 3.2 Positio error equatio r e (t) = r e () + r e (t) + δr e (t) = r e () + δr e (t) = C e c (τ)vc (τ) dτ C e c (τ) (vc (τ) + δv c (τ)) dτ C e c (τ)δv c (τ) dτ δr c (t) = Ce c (t) Cc e (τ)δvc (τ) dτ δṙ c (t) = Ċc e (t) Cc e (τ)δvc (τ) dτ + Ce c (t)ce c (t)δvc (t) = Ċc e (t)δre (t) + δv c (t) = CeΩ c e c/eδr e + δv c = Ω c c/e δrc + δv c = Ω c e/c δrc + δv c δṙ c = ω c e/c δrc + δv c 3.3 Attitude error equatio ω p i/p = ωc i/c + εp ω p i/p = Cp c ωc i/p ( = Cc p ω c i/c + ωc/p) c = Cc p ω c i/c + ω p c/p = (I (ψ )) ω c i/c + ωp c/p = ω c i/c ψ ωc i/c + ωp c/p = ω c i/c ψ ω c i/c + ψ ω c i/c ψ ωc i/c + ψ = ω c i/c + εp ψ = ω c i/c ψ + εp 7

8 3.4 State space error model δ v = ( ω i/e + ω i/) δv ψ f + δf + δθ g + δg δh δṙ = ω e/ δr + δv ψ = ω i/c ψ + εp δgδh = gcomputed gactual = g 2g δh = ξg ηg g 2g δh ξg ηg g + g where ξ ad η are verticle deflectios of gravity, ad g is a bias error i the magitude of gravity. The term 2g δh is the liearized error i the calculated value of gravity due to a error i height. δθ g = = δθ = δr E δr E δr N ta φ δr E ta φ δr N ta φ δr E δr E δr N δr N g δr E g δr E g g δr δθ g + δgδh N + ξg = g δr E + ηg 2g δr D g g δr N ξg = g δr E + ηg 2g δr D g }{{}}{{}}{{} G δr δg = G δr + δg 8

9 ω e/ = ω i/e cos φ ω i/e = ω i/e si φ λ cos φ φ = λ si φ ω i/ = ω i/e + ω e/ v E v N v E ta φ ω i/ + ω i/ = 2ω i/e + ω e/ Combiig all terms for the complete ψ-agle error model: δṙ Ω e/ I ( ) δ v = G 2Ω i/e + Ω e/ (f δr ) ( ) δv + δf + δg ψ Ω i/e + ψ ε p Ω e/ 9

10 1 δṙ N δṙ E δṙ D δ v N δ v E = δ v D ψ N ψ E ψ D λ si φ φ 1 λ si φ λ cos φ 1 φ λ cos φ 1 g (2ω i/e + λ) si φ φ g (2ω i/e + λ) si φ (2ω i/e + λ) cos φ 2g φ (2ω i/e + λ) cos φ f D f E f D f N f E f N (ω i/e + λ) si φ φ (ω i/e + λ) si φ (ω i/e + λ) cos φ φ (ω i/e + λ) cos φ δr N δr E δr D δv N δf N + δg N δv E + δf E + δg E δv D δf D + δg D ψ N ε N ψ E ε E ψ D ε D Figure 2: Complete state space form ψ-agle error equatio.

Σχετικά έγγραφα

EE 570: Location and Navigation

EE 570: Location and Navigation EE 570: Locatio ad Navigatio INS Iitializatio Aly El-Osery Electrical Egieerig Departmet, New Mexico Tech Socorro, New Mexico, USA April 25, 2013 Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25,