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 and Computer Engineering Department Embry-Riddle Aeronautical Univesity Prescott, Arizona, USA April 19, 2016 Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 1 / 10
Overview Position, velocity and attitude drift unless the INS is aided. There are some opportunistic situations that provide information to the INS to initialize itself. Two categories of alignment Coarse Alignment Fine Alignment Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 2 / 10
Self-Alignment 1 Coarse Alignment: Use knowledge of the gravity vector and earth rate provided by the three accelerometers, and the knowledge of the earth rate vector provided by the gyroscopes. 2 Fine Alignment: Needed in quasi-stationary situations. Uses the fact that any position, velocity changes are considered disturbances, and the knowledge of the gravity vector and earth rate to estimate the body s attitude. Latitude needs to be known. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 3 / 10
Coarse Alignment: Approach 1 0 sin(θ) f b = C n b 0 = cos(θ) sin(φ) g g cos(θ) cos(φ) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 4 / 10
Coarse Alignment: Approach 1 0 sin(θ) f b = C n b 0 = cos(θ) sin(φ) g g cos(θ) cos(φ) Only provides pitch and roll angles g (+ve) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 4 / 10
Coarse Alignment: Approach 2 ( f b, ω b, ) f b ω b = Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 5 / 10
Coarse Alignment: Approach 2 ( f b, ω b, ) f b ω b = Ĉ b n ( ) f n, ω n, f n ω n Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 5 / 10
Coarse Alignment: Approach 2 ( f b, ω b, ) f b ω b = Ĉ b n C 11 C 12 C 13 Ĉn b = C 21 C 22 C 23 C 31 C 32 C 33 ( ) f n, ω n, f n ω n Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 5 / 10
Coarse Alignment: Approach 2 where ( f b, ω b, ) f b ω b = Ĉ b n C 11 C 12 C 13 Ĉn b = C 21 C 22 C 23 C 31 C 32 C 33 ( ) f n, ω n, f n ω n ω b b x f C 11 = ω ie cos(l b ) x tan(l b) g C 21 = C 31 = ω y b f b ω ie cos(l b ) y tan(l b) g ω y b f b ω ie cos(l b ) y tan(l b) g C 12 = f b z ωb y f b y ωb z gω ie cos(l b ) C 22 = f b z ωb x + f b x ωb z gω ie cos(l b ) C 32 = f b y ωb x f b x ωb y gω ie cos(l b ) C 13 = f b x g C 23 = f b y g C 33 = f b z g Must ensure that the DCM is properly orthogonalized. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 5 / 10
Fine Alignment Use full INS mechanization Use equivalent to GPS aided error mechanization Setup up measurements Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 6 / 10
Fine Alignment Use full INS mechanization Use equivalent to GPS aided error mechanization Setup up measurements 1 Specific force measurement δ f b = f b ˆ f b Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 6 / 10
Fine Alignment Use full INS mechanization Use equivalent to GPS aided error mechanization Setup up measurements 1 Specific force measurement 2 Angular rate measurement δ f b = f b ˆ f b δ ω b = ω b ˆ ω b Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 6 / 10
Fine Alignment Use full INS mechanization Use equivalent to GPS aided error mechanization Setup up measurements 1 Specific force measurement 2 Angular rate measurement δ f b = f b ˆ f b δ ω b = ω b ˆ ω b 3 Position measurement: deviation from initial position Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 6 / 10
Fine Alignment Use full INS mechanization Use equivalent to GPS aided error mechanization Setup up measurements 1 Specific force measurement 2 Angular rate measurement δ f b = f b ˆ f b δ ω b = ω b ˆ ω b 3 Position measurement: deviation from initial position 4 Velocity measurement: deviation from zero Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 6 / 10
Specific force measurement δ f n = f n ˆ f n = f n (I [ δψ n nb ])C n b ( f b δ f b ) + f d = [ δψ n nb ]C n b f b + Ĉ n b δ f b + f d Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 7 / 10
Specific force measurement Ĉ n b δ f n = f n ˆ f n = f n (I [ δψ n nb ])C n b ( f b δ f b ) + f d = [ δψ n nb ]C b n f b + Ĉ b n δ f b + f d 0 δψ D δψ E 0 = δψ D 0 δψ N 0 + Ĉ b n δ f b + f d δψ E δψ N 0 g Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 7 / 10
Specific force measurement δ f n = f n ˆ f n = f n (I [ δψ n nb ])C n b ( f b δ f b ) + f d = [ δψ nb ]C b n f b + Ĉ b n δ f b + f d 0 δψ D δψ E 0 = δψ D 0 δψ N 0 + Ĉ b n δ f b + f d δψ E δψ N 0 g 0 g 0 δψ N = g 0 0 δψ E + Ĉ b n δ f b + f d 0 0 0 δψ D = G δψ n nb + Ĉ n b δ f b + f b d Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 7 / 10
Specific force measurement δ f n = f n ˆ f n Recoordinatize in the body frame = f n (I [ δψ n nb ])C n b ( f b δ f b ) + f d = [ δψ nb ]C b n f b + Ĉ b n δ f b + f d 0 δψ D δψ E 0 = δψ D 0 δψ N 0 + Ĉ b n δ f b + f d δψ E δψ N 0 g 0 g 0 δψ N = g 0 0 δψ E + Ĉ b n δ f b + f d 0 0 0 δψ D = G δψ n nb + Ĉ n b δ f b + f b d δ f b nb = Ĉ b n G δψ n nb + δ f b + f b d δ f b captures bias-drift (sinking) + Markov,..., and f d represents variations in the nav frame Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 7 / 10
Angular Rate Measurement δ ω n in = ω n in ˆ ω n in Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 8 / 10
Angular Rate Measurement δ ω n in = ω n in ˆ ω n in = (I + [ δψ n nb ])Ĉ n b ( ω b + ω b bn ) Ĉ n b ( ω b δ ω b ) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 8 / 10
Angular Rate Measurement C n b δ ω n in = ω n in ˆ ω n in = (I + [ δψ n nb ])Ĉ n b ( ω b + ω b bn ) Ĉ n b ( ω b δ ω b ) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 8 / 10
Angular Rate Measurement δ ω n in = ω n in ˆ ω n in ˆ ω n = ˆ ω n ie = (I + [ δψ n nb ])Ĉ n b ( ω b + ω b bn ) Ĉ n b ( ω b δ ω b ) Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 8 / 10
Angular Rate Measurement δ ω n in = ω n in ˆ ω n in = (I + [ δψ n nb ])Ĉ n b ( ω b + ω b bn ) Ĉ n b ( ω b δ ω b ) Dev. from stationarity Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 8 / 10
Angular Rate Measurement δ ω n in = ω n in ˆ ω n in = (I + [ δψ n nb ])Ĉ b n ( ω b + ω b bn ) Ĉ b n ( ω b δ ω b ) 0 Ω D 0 δψ N = Ω D 0 Ω N δψ E + Ĉ b n δ ω b ω n d 0 Ω N 0 δψ D = W δψ n nb + Ĉ n b δ ω b ω n d Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 8 / 10
Angular Rate Measurement δ ω n in = ω n in ˆ ω n in Recoordinatize in the body frame = (I + [ δψ n nb ])Ĉ b n ( ω b + ω b bn ) Ĉ b n ( ω b δ ω b ) 0 Ω D 0 δψ N = Ω D 0 Ω N δψ E + Ĉ b n δ ω b ω n d 0 Ω N 0 δψ D = W δψ n nb + Ĉ n b δ ω b ω n d δ ω b in = Ĉ b n W δψ n nb + δ ω b ω b d δ ω b captures bias-drift (sinking) + Markov,..., vcsω d represents variations from stationarity. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 8 / 10
Error State and Measurement Matrix x(t) = F (t) x(t) + w(t) y(t) = H(t) x(t) + v(t) ( x = δψ ) T nb n δ v nb n δ r nb n ba bg 0 3 3 0 3 3 I 3 3 0 3 3 0 3 3 0 H = 3 3 I 3 3 0 3 3 0 3 3 0 3 3 Ĉ n b G 0 3 3 0 3 3 I 3 3 0 3 3 Ĉn b W 0 3 3 0 3 3 0 3 3 I 3 3 where the measurements are: position error, velocity error, specific force error, and angular velocity errors, respectively. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 9 / 10
Challenges There is no mechanism in the above formulation to estimate ω n d. If it can be modelled as white noise then the filter will be able to handle it. On the other hand, if it is correlated type of disturbance, additional measures must be taken to account for it. Aly El-Osery, Kevin Wedeward (NMT) EE 570: Location and Navigation April 19, 2016 10 / 10