Paper Menu >>
Journal Menu >>
Journal of Global Positioning Systems (2006) Vol. 5, No. 1-2:82-87 GPS/INS/Seeker Integrated Navigation System for the Case of GPS Blockage Woo Hyun Kim, Jang Gyu Lee School of Electrical Engineering and Computer Science, Seoul National Un i v e rsity, 133-dong, ASRI #610, San 56-1, Sillim-dong, Gwanak-gu, Seoul, 1 5 1-742, South Korea Hyung Keun Lee School of Electronics, Telecommunication & Computer Eng., Korea Aerospace University, 200-1, Hwajon-dong, De ok yang_gu, Goyang-city, Kyunggi-do, 412-791, South Korea Chan Gook Park School of Mechanical and Aerospace Engi ne ering, Seoul National University, San 56-1, Sillim-dong, Gwanak-gu, Seoul, 151-744, South Korea Abstract. When GPS blockage occurs for a loosely coupled GPS/INS system, its navigation error diverges. To deal with such cases, this paper introduces an integration scheme for GPS, INS, and an image sensor. The proposed integration scheme is attractive in that it accomplished the position and velocity accuracy improvement by the angular information only. The angular information is provided by the gimbal angles of the image sensor. A realistic scenario is studied by a simulation to demonstrate that the GPS/INS/Image integrated navigation system works effectively. Keywords. Image sensor, inertial navigation system (INS), GPS/INS/Image Integrated Navigation System, GPS/INS loosely coupled system 1 Introduction Loosely coupled GPS/INS is one of the most popular integration schemes for modern navigation systems. This scheme performs well when GPS signal conditions are good. However when a GPS signal blockage occurs, its navigation error diverges considerably even within a few minutes. (Maybeck, 1994). GPS blockages occur due to physical obstructions, vehicle dynamics, signal jamming, receiver fault, and satellite faults. Against these cases, additional information is required to compensate the GPS blockage error (Titterton, 1997). For such cases, an image sensor might be a good source of aiding information to prevent the navigation error divergence. In Fig. 1, a conventional configuration for a gimballed image sensor is illustrated. As shown in Fig. 1, the image sensor is suspended by a two-axis gimbal system (Young, 2005). Thus, the pitch and yaw angle measurements regarding the line-of-sight (LOS) information can be provided by the image sensor. For the integration of an INS and an image sensor, a previous study focused on the INS attitude alignment by utilizing a star tracker (Vathal, 1987). Extending the previous research work, this paper investigates the performance improvement of INS by an image sensor in the cases of GPS blockages. As compared with the previous research work that mainly deals with the attitud e accuracy improvement, this paper focuses on the position and velocity accuracy improvement. Fig. 1 Configuration of a two-axis gimballed image sensor (Young, 2005) Kim et al.: GPS/INS/Seeker Integrated Navigation System for the Case of GPS Blockage 83 This paper is organized as follows. At first, important coordinate systems are introduced. Next, measurement equations are derived. Thirdly, a simulation result is demonstrated to validate the performance of the GPS/INS/Image integration scheme. Finally, concluding remarks will be given. 2 Design of GP S/ INS/ I mage integra ted navigation system When an image sensor searches and locks on to a landmark’s image at the center of the image plane, at least 4 frames are necessary to account for this geometry. They are the navigation frame (n-frame), the body frame (b-frame), the image sensor frame (s-frame), and the landmark frame (l-frame) as shown in Fig. 2. The transformation matrixn s C from the image sensor frame to the navigation frame is decomposed into a transformation matrixb s C from the image sensor frame to the body frame and the transformation matrixn b C from the body frame to the navig a tion frame. nnb s bs CCC= (1) As shown in Fig. 2, the unit LOS vector e coincides with the x-direction basis vector s x of the image sensor frame. Eq. (2) expresses the LOS vector e in the image sensor frame. [] 100 T s e= (2) Eq. (3) shows how the LOS vector e with respect to the navigation frame can be obtained by utilizing the transformation matrixn s C. nns s eCe= (3) The exact LOS vectors shown in Eq. (3) cannot be obtained in practice. Instead, an estimate of it can be obtained as follows. ˆˆ nnb s bs CCC=% (4) where () ˆnn bb CC=Ι−Φ (5a) ( ) ˆbb ss CC=Ι−Ε (5b) ˆn s C : estimated transformation matrix from the image sensor frame to the navigation frame ˆn b C : transformation matrix from the body frame to the navigation frame estimated by the INS b s C % : transformation matrix from the image sensor frame to the body frame measured by the gimbal angels Φ : skew symmetric matrix of the INS attitude error Ε : skew symmetric matrix of the gimbal measurement error of the image sensor Fig. 2 Geometry of coordinate systems. The matrixb s C in Eq. (5) is defined as follows coscossincos sin sin coscossin sin sin0 cos s ssss b s sss ss ss C ψ θψψθ ψ θψψθ θθ − ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ − ⎣ ⎦ To construct an indirect Kalman filter, an indirect measurement should be formed as the difference between the raw measurement and the filter’s estimate of its equivalency as follows. . ˆINS meas zy y = −% (6) ˆINS y : value estimate by INS .meas y % : measured value The gimbal angles provided by the image sensor indicate the difference between the body frame and the image sensor frame. Thus, they do not indicate the angles between the body frame and the navigation frame. Due to this characteristic, it was determined that the straightforward derivation like Eq. (6) is difficult and too complex. To simplify this problem, the unit LOS vector n e from the vehicle body to the landmark is utilized as follows. 1) Derive n INS e $. 2) Derive n landmark e $. 3) Subtract n INS e $ from n landmark e $ n INS e $ : unit LOS vector formed by the image sensor 84 Journal of Global Positioning Systems 84 gimbal angle measurements and INS’s attitude information. n landmark e $ : unit LOS vector formed by position of landmark and INS’s position information. Indirect measurement is formed as the difference between n INS e $ and n landmark e $. As a result, the indirect measurement is obtained as follows. ()() LOS ,,, nn landmark INS nn landmark INS Ze e ee fLlhg δδ δδδ =− =− =−ΦΕ $$ ()( )() ()( )( ) ()( )() 111 11 222 22 3333 3 1,11,21,3 000000000 2,12,22,3 000000000 3,13,23,3 000000000 A BC PYPZ A BC PYPZ A BC PYPZ ⎡ ⎤ −− ⎢ ⎥ =−− ⎢ ⎥ ⎢ ⎥ − ⎣ ⎦ [ ] GPS 333333333332 00000ZI ×××××× = GPS GPS-INS-LOS LOS Z ZZ ⎡⎤ =⎢⎥ ⎢⎥ ⎣⎦ (7) As shown above, n landmark e δ is affected by the position error, n INS e δ is affected by the function of vehicle’s attitude error and the gimbal measurement error. For convenience, they are denoted by () ,, f Llh δ δδ and ( ) ,gΦΕ , respectively. 2.1 Derivation of n INS e $ To estimate the LOS vector resolved with respect to the navigation frame, we utilize Eq. (4). For this purpose, the attitude information of the INS and the gimbal angle measurements of the image sensor are utilized. The LOS vector apparently coincides with the x-axis basis vector of the LOS frame. Thus, the LOS vector resolved with respect to the navigation frame can be computed as follows. [] ˆ100 nT n INS s eC= $ (8) The transformation matrix ˆn s C shown in Eq. (8) contains error terms due to Eqs. (5a) and (5b). It is summarized as follows. ˆnn n s ss CC C δ =+ ˆ ˆˆ nnn sss nn b s bs CCC CC C δ =− =−Ε−Φ % (9) Combining Eqs. (3), (8), and, (9), n I NS e δ can be derived as follows. nnn INS INS INS ee e δ =+ $ [] () 100 , T nn INS s eC g δδ = =ΦΕ (10) [][][][][ ] [][][][][ ] [][][][][ ] () () () 11111 222 2 2 333 3 3 1,1 2,1 3,1 zy zy zy CABPZPY CABPZPY CABPZPY δγδα δβφφ δγδα δβφφ δγδαδβ φφ ⎡ ⎤ +−+ − ⎢ ⎥ =−+− +− ⎢ ⎥ ⎢ ⎥ +++ − ⎣ ⎦ ⎡⎤ ⎢⎥ =−⎢⎥ ⎢⎥ ⎣⎦ As shown in Eq. (10), n I NS e δ is a 3-by-1 matrix. Denoting ,,,, yz δ αδβδγφ φ as the roll error, pitch error, yaw error, gimbal pitch angle error, and gimbal yaw angle error, respectively, the elements of n INS e δ are summarized as follows. () [sin sincoscos sin][cos cos] [coscos][cos sin] [cos sincossinsin][cos sin] [sinsincoscossin][sin ] [coscos][sin ] [cos sincos 1,1 bbbb bss bb ss bbbbbs s bbbb bs bb s bb φθ ψφψθ ψ δγ θψ θψ φθ ψφψθψ δα φθ ψφψθ θψ θ δβ φθ ψ − ⎡⎤ ⎢⎥ − ⎣⎦ + ⎡⎤ +⎢⎥ +− ⎣⎦ −+ = sin sin][cos cos] [coscos][ sin] [sinsincoscossin] [] [cos ] [cos cos][sin cos ] [sin sincoscos sin] [sin sin] [cos sin bbbss bb s bbbb bz s bb ss bbbb b ss b φψθ ψ θψ ψ φθ ψφψφ ψ θψ θψ φθ ψφψ θψ φ ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎡ ⎤ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ + ⎣ ⎦ ⎝⎠ − ⎛⎞ ⎜⎟ +− ⎜⎟ ⎜⎟ ⎝⎠ ++− − + [] cossin sin] [cos ] y bb bb s φ θψ φψ θ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎛⎞ ⎢ ⎥ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎛⎞ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎢ ⎥ + ⎜⎟ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎜⎟ ⎢ ⎥⎝⎠ ⎢ ⎥ ⎝⎠ ⎣ ⎦ () [sin sinsincos cos][cos cos] [cos sin][cos sin] [cossinsin sincos][cossin] [sinsinsincoscos][sin ] [cossin][sin ] [cos sinsin 2,1 bb bbbss bb ss bb bbbss bb bbbs bb s bb φθψφψ θψ δγ θψ θψ φθψφψ θψ δα φθψφψ θ θψ θ δβ φθψ + ⎡⎤ ⎢⎥ − ⎣⎦ − ⎡⎤ +⎢⎥ ++ ⎣⎦ −+ = sin cos][cos cos] [cossin][ sin] [sinsinsincoscos] [] [cos ] [cos sin][sin cos] [sin sin sincos cos] [sin sin] [cos sin bbbss bb s bb bbbz s bbs s bb bbb ss b φψ θψ θψ ψ φθψ φψφ ψ θψ θψ φθψ φψ θψ φ ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎡ ⎤ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ − ⎣ ⎦ ⎝⎠ − ⎛⎞ ⎜⎟ ++ ⎜⎟ ⎜⎟ ⎝⎠ +++ − + [] sinsin cos] [cos ] y bbb b s φ θψ φψ θ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎛⎞ ⎢ ⎥ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎛⎞ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎢ ⎥ − ⎜⎟ ⎜⎟ ⎢ ⎥ ⎜⎟ ⎜⎟ ⎜⎟ ⎢ ⎥⎝⎠ ⎢ ⎥ ⎝⎠ ⎣ ⎦ Kim et al.: GPS/INS/Seeker Integrated Navigation System for the Case of GPS Blockage 85 () [sincos ][coscos] [sin ][cossin] [coscos ][cossin] [sincos ][sin ] [sin][sin ] [coscos ][coscos] 3,1 [sin ][sin] [sin bbs s bss bb ss bbs bs bbs s bs b φθ θψ δγ θθψ φθ θψ δα φθ θ θθ δβ φθ θψ θψ φ ⎛⎞ ⎡⎤ ⎜⎟ ⎢⎥ + ⎣⎦ ⎜⎟ ⎜⎟ ⎡⎤ ⎜⎟ +⎢⎥ + ⎜⎟ ⎣⎦ ⎜⎟ ⎡⎤ ⎜⎟ ⎢⎥ ⎜⎟ ⎜⎟ − =⎣⎦ ⎝⎠ + + [] cos ][cos] [sin ][sincos] [sincos][sinsin] [] [coscos][cos ] z bs bss bb ssy bbs φ θψ θθψ φθ θψφ φθθ ⎡⎤ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎛⎞ ⎛⎞ ⎢⎥ ⎜⎟ ⎜⎟ ⎢⎥ ⎝⎠ ⎜⎟ ⎢⎥ ⎜⎟ − ⎛⎞ ⎢⎥ ⎜⎟ ⎜⎟ ⎢⎥ −+⎜⎟ ⎜⎟ ⎢⎥ ⎜⎟ ⎜⎟ ⎜⎟ + ⎢⎥ ⎢⎥ ⎝⎠ ⎝⎠ ⎣⎦ 2.2 Derivation of n lan d ma rk e $ If the exact positions of the landmark and the vehicle are known, the unit LOS vector n e can be obtained, differently from (3), as follows. ( ) 1 nnn eDD − = (11) nn L DCL=∆ (12) Where Eq. (12) denotes the position difference between the landmark and the vehicle. As shown in Eq. (12), it is the function of the differences in latitude, longitude, and height. [] [] TT landmark vehicle LLlhLlh∆=− (13) The scaling matrix n L C shown in Eq. (12) is purposed to transform the latitude, longitude, and, height differences to the difference vector resolved with respect to the locally-level navigation frame. () 00 0cos0 001 m n Lt Rh CRhL ⎡⎤ + ⎢⎥ =+ ⎢⎥ ⎢⎥ − ⎣⎦ (14) In Eq. (14), m R denotes the meridian radius of curvature and t R denotes the transverse radius of curvature of the earth. Since it is hard to obtain the exact po sition of vehicle and the exact position of vehicle is an object of estimation, it is not available in practice. Instead , th e estimated latitu d e, longitude, and height info rmation of the INS is utilized as follows. () 1 ˆˆ nnn landmark eDD − = $ (15) ˆˆ nn L DCL = ∆ (16) $ [] $ $ T T landmark I NS LLlh Llh ⎡ ⎤ ∆= − ⎣ ⎦ $ (17) Combining Eqs. (16) and (17), the following relationshi p s c a n be derived. [] ˆT nn n LL INS DCLCLlh δδδ =∆− ˆnnn D DD δ =+ (18) [] T nn LINS DCLlh δδδδ =− (19) [] $ [] ˆT TT INS vehicle INS LlhLlh Llh δδδ ⎡⎤ =− ⎣⎦ $ (20) The estimated LOS vector n landmark e $ can be obtained by applying the perturbation method to Eqs. (11) and (15) with having such error sources. nnn landmark landmark landmark ee e δ =+ $ ˆˆ ... ˆ ˆˆ n nnn nn landmark n nnn D DD D eDHOT D DD D δ ⎛⎞ ∂⎜⎟ ==+ + ⎜⎟ ∂⎝⎠ $ ˆ ˆˆ n n nn landmark nn D D eD DD δ δ ⎛⎞ ∂⎜⎟ =⎜⎟ ∂⎝⎠ 33 3 ˆˆˆ 1 ˆˆˆ ˆ nnnT nnn n DDD I DDD D × ⎛⎞ ∂⎜⎟ =− ⎜⎟ ∂⎝⎠ () () 1 13 33 ˆˆˆˆ ,, nnnnTnn landmark eDDDDDfLlh δ δδδδ − − × ⎛⎞ ⎛⎞ ≈Ι −= ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ ⎝⎠ [] ()( )() ()( )() ()()() _ 1,11, 21, 3 2,12,22,3 3,13, 23, 3 T nn landmark landmarkINS INS epriorie Llh L l h δδδδδ δ δ δ =× ⎡⎤ ⎡⎤ ⎢⎥ ⎢⎥ =⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎣⎦ ⎣⎦ (21) 2.3 Handling rank deficiency As previously explained, the image sensor provides two direct measurements, i.e., pitch and yaw gimbal angles. However, the indirect measurement formed by the direct measurements is 3-by-1 vector. Thus, a rank deficiency can occur, which means that one element is a linear combination of the other two elements. Since the Kalman filter assumes independent measurements, the rank deficiency should be eliminated. 86 Journal of Global Positioning Systems 86 To eliminate the rank deficiency, the observation matrix H should be decomposed into two parts; INS part and image sensor part. ()()() ()( )() ()( )() 111 11 222 22 33333 1,11,21,3 000000000 2,12,22,3 000000000 3,13,23,3 000000000 ABC PYPZ H ABC PYPZ ABC PYPZ ⎡ ⎤ −− ⎢ ⎥ =−− ⎢ ⎥ ⎢ ⎥ − ⎣ ⎦ ()( )() ()()() ()()() 111 222 333 1,11,21,3000000000 2,12,22,3 000000000 3,13,23,3 000000000 INS ABC HABC ABC ⎡⎤ − ⎢⎥ =− ⎢⎥ ⎢⎥ ⎣⎦ 11 LOS2 2 33 PY PZ H PY PZ PY PZ ⎡⎤ − ⎢⎥ =− ⎢⎥ ⎢⎥ − ⎣⎦ (22) Meanwhile, by the singular value decomposition, the image sensor part observation matrix can be rewritten as follows. LOS T H USV= T UU I= T VV I= (23) Where U and V denote orthonormal matrix and S denotes quasi-diagonal matrix with the singular value as its diagonal elements, respectively. If we denote B as the first two columns of the orthonormal matrix U, it can be verified that T B His a full rank matrix and the following relationships are satisfied. LOS LOS T ZBZ= ( T H BH= ( LOS LOS T HBH= ( () 1 LOSLOS LOS 1 LOS LOS TT ZHZ BH BZ − − = = ) (( (24) It can be verified that LOS Z ) shown in Eq. (24) is a 2-by-1 vector and is free of rank deficiency. As a result, the Kalman filter utilizes the following full rank measurement vector instead of that shown in Eq. (7). -- LOS GPS GPS INS LOS Z ZZ ⎡⎤ =⎢⎥ ⎢⎥ ⎣⎦ ) (25) 3 Simulation a nd result To assess the performance of the GPS/INS/Image integrated navigation system, a simulation has been performed. For the simulation, the initial latitude error, longitude error, and height error are set as 40/Ro[rad], 40/Ro[rad], and 40[m], respectively, where Ro denotes the mean radius of the earth. The initial velocity error in each direction is set as 10[m/sec], respectively. The initial roll error of vehicle, pitch error of vehicle, and yaw err or of vehicle are set as 1[deg], 1[deg], and, 5[deg], respectively. The scale factor, misalignment error, white noise, and, random constant of the accelerometer are set as 200[ppm], 10[arcsec], 5[ g µ ], and 100[ g µ ], respectively. It is assumed that GPS blockage occurs at from 121[sec] to 180[sec] during the flight. To enhance the observability, an S-shape turn trajectory is applied to vehicle trajectory. The state variables used for the loosely-coupled GPS/INS system are the position errors, velocity errors, attitude errors, accelerometer bias, and, gyro drift (Young Bum Park, 2001). The proposed GPS/INS/Image integrated navigation system augments the conventional 15 error states by the two bias values in the image sensor’s pitch and yaw measurements. 111233 33 3333 21222324333 3 3132333 3353 3 GPS-INS-LOS 33 33 33 33 3333 33 33 33 33 3333 22 22 22 22 2266 0000 00 00 000000 000000 00000 FF FFFF FFF F F F ×××× ×× ×× ×××××× ×××××× ××××× ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ T LOS T T INS XX X δδ δ ⎡ ⎤ = ⎣ ⎦ [ ] INS T INSINSINSNEDXYZX Y Z X LlhVVV δ δ δδ δδδδαδβδγεεε = ∇∇∇ LOS T yz X δ φφ ⎡ ⎤ = ⎣ ⎦ Fig. 3, Fig. 4, Fig. 5 show position, velocity, and, attitude errors of the two integration schemes, respectively. As shown in all figures, the GPS/INS loosely coupled system shows an accumulation of navigation error during the GPS blockage between 121[sec] and 180[sec]. As compared, the proposed GPS/INS/Image integrated navigation system generates a trajectory which is close to the truth trajectory during all the time. Thus, it can be concluded that it is possible to bound all the navigation errors by utilizing only the two angle measurements. It is interpreted that the error bounding is possible tthrough the direct range information between the vehicle and the landmark is not available. The attractive error bounding feature is interpreted due to the observability enhancement brought by the vehicle’s S-turn. Kim et al.: GPS/INS/Seeker Integrated Navigation System for the Case of GPS Blockage 87 Fig. 3 Comparison position err ors. Fig. 4 Comparison velocity errors. Fig. 5 Comparison attitude errors. 4 Conclusions The paper proposed a new GPS/INS/Image integration scheme to prevent the INS error accumulation during GPS blockages. For the purpose, two gimbal measurements of the image sensor are utilized. Assuming that the image sensor acquires a known landmark in the center of its image plane, a simulation was performed. By the simulation result, it was confirmed that it is possible to bound all the nav igation error s by utilizing the image sensor’s two angle measurements only. Acknowledgements This work has b een suppor ted by the Agency for Defence Development (ADD) and the Automatic Control Research Center (ACRC), Seoul National University, South Kor ea. References Maybeck P.S. (1994) Stochastic Models, Estimation, and Control, Vol.1, Navtech Book & Software Store pp.291~297. Park Y.B. (2001) Design of Integrated INS/GPS/Odometer Navigation System and Its Performance Analysis, School of Electronical Engineering and Computer Science, Seoul National University. Titterton D.H., Weston J.L. (1997) Strapdown inertial navigation technology, Peter Peregrinus Ltd., pp.363~394. Vathal S. (1987) Spacecraft Attitude Determination Using a Second-Order Nonlinear Filter, Journal of Guidance and Control, Vol.10, No 6, pp.559~565. Young R.M. (2005) Feedback Linearization Control of a 2- Axis Gimbal Seeker in BTT Missile, School of Electronical Engineering and Computer Science, Seoul National University. |