EE 570: Location and Navigation

Μέγεθος: px

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

Download "EE 570: Location and Navigation"

Αἴσων Αυγερινός
5 χρόνια πριν
Προβολές:

1 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, / 10

2 Overview Positio, velocity ad attitude drift uless the INS is aided. There are some opportuistic situatios that provide iformatio to the INS to iitialize itself. Two categories of aligmet Coarse Aligmet Fie Aligmet Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

3 Self-Aligmet 1 Coarse Aligmet: Use kowledge of the gravity vector ad earth rate provided y the three accelerometers, ad the kowledge of the earth rate vector provided y the gyroscopes. 2 Fie Aligmet: Needed i quasi-statioary situatios. Uses the fact that ay positio, velocity chages are cosidered disturaces, ad the kowledge of the gravity vector ad earth rate to estimate the ody s attitude. Latitude eeds to e kow. Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

4 Coarse Aligmet: Approach 1 f i = C 0 0 = g Oly provides pitch ad roll agles g (+ve) si(θ) cos(θ) si(φ) cos(θ) cos(φ) g Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

5 Coarse Aligmet: Approach 2 where ( f i, ω i, f i ω ) i = Ĉ C 11 C 12 C 13 Ĉ = C 21 C 22 C 23 C 31 C 32 C 33 ( ) f i, ω i, f i ω i C 11 = C 21 = C 31 = ω x ω ie cos(l ) f x ta(l ) g ω y f ω ie cos(l ) y ta(l ) g ω y f ω ie cos(l ) y ta(l ) g C 12 = f z ω y f y ω z gω ie cos(l ) C 22 = f z ω x + f x ω z gω ie cos(l ) C 32 = f y ω x f x ω y gω ie cos(l ) C 13 = f x g C 23 = f y g C 33 = f z g Must esure that the DCM is properly orthogoalized. Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

6 Fie Aligmet Use full INS mechaizatio Use equivalet to GPS aided error mechaizatio Setup up measuremets 1 Specific force measuremet 2 Agular rate measuremet δ f i = f i ˆ f i δ ω i = ω i ˆ ω i 3 Positio measuremet: deviatio from iitial positio 4 Velocity measuremet: deviatio from zero Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

7 Specific force measuremet δ f i = f i ˆ f i = f i (I [ δψ ])C ( f i δ f i ) = [ δψ ]C fi δ f i 0 δψ D δψ E 0 δψ D 0 δψ N 0 + Ĉ δ f i + fd δψ E δψ N 0 g 0 g 0 δψ N g 0 0 δψ E + Ĉ δ f i + fd δψ D = G δψ + Ĉ δ f i + fd Recoordiatize i the ody frame δ f = Ĉ G δψ + δ f i + f d δ f i captures ias-drift (sikig) + Markov,... Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

8 Specific force measuremet Ĉ δ f i = f i ˆ f i = f i (I [ δψ ])C ( f i δ f i ) = [ δψ ]C fi δ f i 0 δψ D δψ E 0 δψ D 0 δψ N 0 + Ĉ δ f i + fd δψ E δψ N 0 g 0 g 0 δψ N g 0 0 δψ E + Ĉ δ f i + fd δψ D = G δψ + Ĉ δ f i + fd Recoordiatize i the ody frame δ f = Ĉ G δψ + δ f i + f d δ f i captures ias-drift (sikig) + Markov,... Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

9 Agular Rate Measuremet δ ω i = ω i ˆ ω i = (I +[ δψ ])Ĉ ( ω i + ω ) Ĉ ( ω i δ ω i ) 0 Ω D 0 δψ N Ω D 0 Ω N δψ E + Ĉ δ ω i ω d 0 Ω N 0 δψ D = W δψ + Ĉ δ ω i ω d Recoordiatize i the ody frame δ ω i = Ĉ W δψ + δ ω i ω d δ ω i captures ias-drift (sikig) + Markov,... Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

10 Agular Rate Measuremet δ ω i = ω i ˆ ω i C = (I +[ δψ ])Ĉ ( ω i + ω ) Ĉ ( ω i δ ω i ) 0 Ω D 0 δψ N Ω D 0 Ω N δψ E + Ĉ δ ω i ω d 0 Ω N 0 δψ D = W δψ + Ĉ δ ω i ω d Recoordiatize i the ody frame δ ω i = Ĉ W δψ + δ ω i ω d δ ω i captures ias-drift (sikig) + Markov,... Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

11 Agular Rate Measuremet δ ω i = ω i ˆ ω i ˆ ω i = ˆ ω ie = (I +[ δψ ])Ĉ ( ω i + ω ) Ĉ ( ω i δ ω i ) 0 Ω D 0 δψ N Ω D 0 Ω N δψ E + Ĉ δ ω i ω d 0 Ω N 0 δψ D = W δψ + Ĉ δ ω i ω d Recoordiatize i the ody frame δ ω i = Ĉ W δψ + δ ω i ω d δ ω i captures ias-drift (sikig) + Markov,... Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

12 Agular Rate Measuremet δ ω i = ω i ˆ ω i = (I +[ δψ ])Ĉ ( ω i + ω ) Ĉ ( ω i δ ω i ) 0 Ω D 0 δψ N Dev. from statioarity Ω D 0 Ω N δψ E + Ĉ δ ω i ω d 0 Ω N 0 δψ D = W δψ + Ĉ δ ω i ω d Recoordiatize i the ody frame δ ω i = Ĉ W δψ + δ ω i ω d δ ω i captures ias-drift (sikig) + Markov,... Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

13 Error State ad Measuremet Matrix x(t) = F(t) x(t)+ w(t) y(t) = H(t) x(t)+ v(t) ( ) T x = δ ψ δ v δ r a g I H = 3 3 I Ĉ G I ĈW I 3 3 Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

14 Challeges There is o mechaism i the aove formulatio to estimate ω d. If it ca e modelled as white oise the the filter will e ale to hadle it. O the other had, if it is correlated type of disturace, additioal measures must e take to accout for it. Aly El-Osery (NMT) EE 570: Locatio ad Navigatio April 25, / 10

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

EE 570: Location and Navigation

EE 570: Location and Navigation INS Initialization Aly El-Osery Kevin Wedeward Electrical Engineering Department, New Mexico Tech Socorro, New Mexico, USA In Collaboration with Stephen Bruder Electrical