Distributed data fusion algorithms for inertial network systems

Transcription

1 Dstrbuted data fuson algorthms for nertal network systems D.J. Allerton and H. Ja Abstract: New approaches to the development of data fuson algorthms for nertal network systems are descrbed. The am of ths development s to ncrease the accuracy of estmates of nertal state vectors n all the network nodes, ncludng the navgaton states, and also to mprove the fault tolerance of nertal network systems. An analyss of dstrbuted nertal sensng models s presented and new dstrbuted data fuson algorthms are developed for nertal network systems. The dstrbuted data fuson algorthm comprses two steps: nertal measurement fuson and state fuson. The nertal measurement fuson allows each node to assmlate all the nertal measurements from an nertal network system, whch can mprove the performance of nertal sensor falure detecton and solaton algorthms by provdng more nformaton. The state fuson further ncreases the accuracy and enhances the ntegrty of the local nertal states and navgaton state estmates. The smulaton results show that the two-step fuson procedure overcomes the dsadvantages of tradtonal nertal sensor algnment procedures. The slave nertal nodes can be accurately algned to the master node. 1 Introducton The concept of an nertal network system n arcraft avoncs was ntally proposed by Kelley et al. [1] and subsequently developed by Bernng et al. [] and Kaser et al. []. Inths archtecture, nertal sensor systems are located at several places n an arcraft n order to meet the fault tolerance requrements of arcraft navgaton and to provde accurate local nertal state vectors for several arborne avoncs systems. For example, fre/weapon control systems and radar search/trackng systems requre accurate local and centre of gravty ()-referenced nertal state nformaton to stablse these systems and to compensate for local moton. The development of nertal network systems arse from relablty requrements and ncreased relance on nertal nformaton, partcularly n advanced mltary arcraft where arframe flexblty must be taken nto account n the nstallaton and algnment of arborne avoncs systems. Inertal sensor systems provde flght-crtcal nformaton for all safety or msson-crtcal avonc systems, such as flght control and navgaton systems, as well as other arborne systems. Redundant nertal systems are used to provde the level of fault tolerance necessary n arcraft navgaton systems, n order to meet safety and relablty requrements for cvl or mltary arcraft. Typcally, a combat platform may have 1 nertal measurement unts (IMUs) of varous qualty provdng the nertal state vector nformaton needed n msson-crtcal avoncs systems and weapon systems []. These IMUs are nstalled at dfferent locatons n a flexble arframe and both # The Insttuton of Engneerng and Technology 008 do: /et-rsn: Paper frst receved rd November 00 and n revsed form th August 00 D.J. Allerton s wth the Department of Automatc Control and Systems Engneerng, The Unversty of Sheffeld, Mappn Street, Sheffeld S1 JD, UK H. Ja s wth the Department of Aerospace Engneerng, Cranfeld Unversty, Cranfeld, Bedfordshre MK4 0AL, UK E-mal: d..allerton@sheffeld.ac.uk structural and n-flght msalgnments between these sensors/weapon locatons need to be estmated n order to algn the dynamc sensors and weapon systems. For example, the performance of sensors/systems such as SAR and terran followng radar, pontng systems, forward-lookng nfrared recevers, laser spot trackers and mssle pylons all depend on the precse algnment of the system senstve axes n nertal networks. Fuson of measurements from dstrbuted IMUs can provde hghly relable nertal vector nformaton and can also be used to detect sensor falures and to reconfgure nertal measurements n nertal networks. Durng the last few years, dstrbuted and ntegrated modular avoncs archtectures have been ntroduced nto modern arcraft systems [4, 5] as a result of advances n hgh-speed arborne data communcaton networks and embedded computer systems. These enablng technologes provde arborne avoncs systems wth powerful data processng and communcaton capablty. Furthermore, the reducng cost, sze and mass of sensors, ncludng fbre optc gyros and mcroelectromechancal systems (MEMS) nertal sensor systems [, ], have enabled redundant nertal sensors to be ntegrated nto a sngle IMU box. In these systems, non-orthogonal confguratons can be used to mprove system relablty and to reduce the cost, sze and mass of arcraft navgaton systems. In a non-orthogonal confgured IMU, redundant nertal sensors are skewed wth respect to the orthogonal frame; ths confguraton s known as a skewed redundant IMU (SRIMU). Few researchers to date have focused on a systematc approach to the desgn of nertal network algorthms. Ths paper proposes nnovatve data fuson methods for nertal network systems to provde dynamc algnment and calbraton of dstrbuted nertal systems. Inertal network system archtectures An nertal network archtecture s llustrated n Fg. 1 n whch each node represents an ndvdual sensng locaton IET Radar Sonar Navg., 008,, (1), pp

2 Fg. 1 Relatonshps between IMU nodes consstng of an IMU and an embedded mcroprocessor. Each node s assumed to be able to communcate wth other nodes, so that nformaton from one node can be shared by other nodes. The IMU at each node can be ntegrated wth other navgaton-adng systems, ncludng global navgaton satellte systems (GNSS), Doppler radar and other rado navgaton systems. Avoncs data buses are used to nterconnect the IMU nodes to mplement an nertal network system. The node located at the arcraft s usually a master node, also referred to as the node; the other nodes are the local nodes, known as slave nodes. The data fuson flter located at the node provdes the navgaton states and the -referenced nertal state vector; the data fuson flters located at the slave nodes provde the local states and the local nertal vector nformaton. Ths form of dstrbuted nertal network system [1] s extended n ths paper and affords the followng advantages. 1. Fault tolerance and robustness to sensor/system falures. Data fuson algorthms are desgned so that the falure of any node or element of the node wll not lead to degradaton of the performance of the arcraft navgaton system. Moreover, degradaton of the performance of local systems at a faled node wll be gradual.. Flexblty. It s straghtforward to add or remove one or more sensor systems n a dstrbuted system network.. Hghly relable state estmaton. The data fuson flter combnes all local estmates wth ts own estmate to obtan the arcraft moton states, whch are used to support arcraft navgaton, flght control and gudance and other functons that requre -referenced data. 4. Accurate local state estmaton. The slave data fuson flter located at each node fuses all measurements from healthy nertal sensor systems to derve globally optmal estmates of the local states; these are used to support the stablsaton of varous avoncs system platforms and to provde local moton compensaton. 5. Automatc algnment of low-qualty sensors. Because nformaton s shared by all nodes, dstrbuted data fuson flters can use the local estmates obtaned at a node wth a hgh-qualty IMU, to dynamcally correct and algn lowqualty IMUs stuated at other nodes. Consequently, tradtonal nertal system algnment algorthms, whch may necesstate specfc manoeuvers to be flown, are no longer necessary n dstrbuted nertal network systems. 5 Dstrbuted nertal sensng models.1 Dstrbuted node frames Although IMUs n an nertal network ndependently measure ndvdual local qualtes, the measured or estmated states are not completely ndependent; they are dynamcally related to each other owng to the rgd structure of the arcraft. The development of ths dynamc relatonshp between the local measured states enables the nertal nformaton provded by an nertal network to be used to detect and solate sensor/system falures and to mplement the dynamc algnments between dfferent nertal systems. Fg. 1 llustrates three local IMU frames and the correspondng local reference frames. Let I be an nertal reference frame, let be the local body frame of the IMU node located at the arcraft centre of gravty and represent the slave IMU nodes and ther ndvdual local body frames. In ths paper, the IMU frames are assumed to be algned wth the local body frames when the IMUs are nstalled at local locatons n the arcraft.? represents a translatonal transformaton, for example, the translaton vector from nodes to s denoted by?. T represents a rotatonal transformaton, for example, the transformaton matrx from nodes to s denoted by T. Exchangng the superscrpt and the subscrpt of a transformaton matrx represents an nverse of ths transformaton, for example, (T ) 1 ¼ T. Let the local reference frames at the nodes, and be denoted by L, L and L, respectvely, then the correspondng transformatons from the local reference frames to the local body frames are gven by T L, T L and T L. If the local level frames, such as the north-east-down, are used as the local reference frames, these rotaton matrces represent the orentatons of the local body axes relatve to the local level frames. Because the orgns of the local level frames are defned wth respect to the geodetc coordnates of the IMU nodes and the magntudes of the translaton vectors between these IMU nodes are very small, the msalgnments between the local level frames, caused by the translaton vectors, can be gnored. In order to smplfy the development of an nertal network sensng model, t s assumed that the local-level frames located at all the nertal network nodes are dentcal, that s L = L = L. Let the relatve rotaton of the IMU frame wth respect to the frame be v = and ts nverse rotaton be v = ¼ v =. From mult-body rotaton mechancs, the IET Radar Sonar Navg., Vol., No. 1, February 008

3 absolute angular velocty of an IMU frame n an nertal network system s the sum of the absolute angular velocty of another IMU frame and the relatve angular velocty between these two frames. Therefore, the absolute angular rate vectors measured by the local IMUs can be represented as follows v I= ¼ v I= þ v = (1) v I= ¼ v I= þ v = () v I= ¼ v I= þ v = () The rotatonal transformatons between the local IMU frames depend on the relatve angular veloctes between these frames. In the statonary (rgd arframe) case, there s no relatve angular moton between the IMU nodes; n the dynamc case, the relatve angular rates between dfferent IMU frames can be treated as a random varable.. Statonary nertal sensng model In ths model, the arcraft structure s assumed to be rgd. The dynamc relatonshps between dfferent IMU frames can be descrbed by fxed rotatonal and translatonal transformatons, whch can be precsely determned from the geometry of IMU locatons at the tme of nstallaton or subsequently estmated usng a dstrbuted algnment Kalman flter. If the local state x s a rate vector (such as acceleraton, velocty or angular velocty), a rotaton matrx s used to complete the rotaton transformaton between node frames, for example, from nodes to, as follows x ¼ T x (4) If the local state x s a dsplacement vector, a rotaton matrx s combned wth a translaton vector to represent the transformaton between frames as follows x ¼ T x þ? (5) where the states x and x are expressed n ther ndvdual local frames, T a known rotaton matrx and? a known translaton vector expressed relatve to node. For the case where the local states are the Euler angles, the atttude matrx transformaton from one node to the other has the followng form T L ¼ T T L () It should be noted that the new Euler angles n () are computed from the matrx T L. If a node IMU s an SRIMU, the measured nertal states, acceleratons and angular rates, are expressed n terms of the local body frame but the IMU outputs are represented n the nertal nstrument frame, whch defnes the sensng drectons of nertal sensors and dffers from the IMU frame. The transformaton between the nertal nstrument frame and the local IMU frame, for example, at the node, s gven by H mu where the subscrpt mu represents the nertal nstrument frame. H mu (referred to as the desgn matrx) depends on the SRIMU confguraton and can be dynamcally reconfgured [8, 9]. The SRIMU measurement vector at the node,, can be rewrtten as follows ¼ H mu x () By applyng rotaton transformatons, the measurement vectors of SRIMUs located at nodes and, and, can be represented n terms of the local body frame at the node as follows ¼ H mu x ¼ H mu T x (8) ¼ H mu x ¼ H mu T x (9) where H mu and H mu are the SRIMU desgn matrces at nodes and, respectvely. From (8) and (9), the node assmlates the nertal measurement nformaton from the slave nodes and. Therefore the total nertal measurement at the node s represented as H mu m ; 4 5 ¼ H mu T 4 5 x ¼ H x (10) H mu T Smlarly, the nertal measurement vectors at nodes and are as follows m ; 4 m ; 4 5 ¼ 5 ¼ 4 4 H mu H mu T H mu T H mu T H mu T H mu 5 x ¼ H x (11) 5 x ¼ H x (1) Equatons (10) (1) ndcate that each node shares the same redundant nertal measurements even though all the IMUs are tradtonal orthogonal systems. Varous weghted least-squares estmators can be appled to the redundant measurement equatons to estmate the local nertal states. Ths data assmlaton and weghted least-squares estmaton s referred to as nertal measurement (data) fuson n ths paper. Furthermore, many falure detecton and solaton algorthms, such as the party space-based methods and generalsed lkelhood rato test algorthms descrbed n [8, 10, 11], can be appled to these equatons to detect and solate nertal sensor falures n the nertal network system. As a result, the use of nertal measurement fuson procedures ncreases the accuracy of the local nertal state estmates at each IMU node and mproves the performance of the navgaton system. From (), one IMU node can also assmlate atttude nformaton from other IMU nodes usng the followng transformatons. At node, the local atttude matrx s The assmlated atttude matrces are T L (1) T L ¼ T T L (14) T L ¼ T T L (15) As a result, the redundant atttude nformaton at node conssts of (1) (15). At node, the local atttude matrx s T L (1) IET Radar Sonar Navg., Vol., No. 1, February 008 5

4 The assmlated atttude matrces are T L ¼ T T L (1) T L ¼ T T L (18) Smlarly, the redundant atttude nformaton at node conssts of (1) (18). At node, the local atttude matrx s T L (19) The assmlated atttude matrces are T L T L ¼ T T L (0) ¼ T T L (1) where the redundant atttude nformaton at node conssts of (19) (1). The redundant atttude matrx nformaton at each node can be fused by a weghted least-squares estmator to ncrease the accuracy of the local atttude estmates and to mprove the fault tolerance of the navgaton states.. Dynamc nertal sensng model Although the assumpton of a rgd body arcraft apples to a wde range of applcatons n arcraft navgaton and control systems, ths assumpton may be nvald n many mltary applcatons where precse local nertal states are needed ncludng targetng systems, pontng systems and other weapon systems. Durng hgh-speed flght and hghdynamc manoeuvres, the arframe should be consdered as a flexble structure. The rotatonal matrces gven n the prevous secton are no longer statonary but are tmevaryng dynamc rotaton matrces. If the flexble structure of an arcraft s gnored, these matrces ntroduce errors n the rotaton transformaton, leadng to large errors n the estmates of the local states. Accordngly, t s necessary to develop the dynamc relatonshps between the network nodes and to estmate these dynamc transformaton matrces n flght. Carson et al. [1] suggest a dfferental nertal flter (DIF) to estmate the angular flexng of the slave IMU frames wth respect to a reference IMU frame, usually the IMU node frame. The DIF method processes a dfferental or delta nertal state vector, whch s the dfference between the and slave IMU measurements. However, ths method depends on two vtal condtons. Frst, compensaton s needed for the lever-arm acceleratons and flexng angular rates before ntatng the DIF. For sgnfcant flexng moton of an arframe, ths compensaton s dffcult to compute, partcularly durng manoeuvres because the flexng of the slave IMU node frames (wth respect to the frame) changes wth flght condtons. Secondly, the DIF dynamc model s partcularly senstve to arcraft manoeuvres and the resultant flexng moton. Two methods are presented n ths paper to determne these dynamc rotatonal transformatons, whch avod compensaton for lever-arm acceleratons and flexng angular rates between the and the slave IMU nodes. The frst approach s an teratve estmaton method and the second method establshes analytcal models of the rotaton matrces. In both methods, t s assumed that the ntal transformaton matrces are known. In practce, these matrces can be determned from the statonary transformaton matrces descrbed n the prevous secton, when the arcraft s on the ground or n level non-acceleratng flght Iteratve estmatng method: The teratve estmaton method s based on () where the local atttude matrces are computed at all the IMU nodes by nvokng the nertal atttude determnaton algorthm. The dynamc transformaton matrces are then estmated from the computed local atttude matrces. Because the dynamc change of the rotaton matrx relatve to ts ntal matrx s generally wthn a small dynamc range, the current estmate of the rotaton transformaton T k can be approxmated by the combnaton of the prevous estmate and a small angle dsplacement vector c k. The estmated rotaton matrx at the current tme can be rewrtten as ^T k ^T k 1 (I þ c k ) ¼ ^T L ^T L where the subscrpt k represents the teraton step. Therefore the teratve computaton equaton can be gven as follows (I þ c k ) ¼ [ ^T k 1 ] 1 ^T L L ^T () The method s outlned n Fg.. When the norm of c k s less than a specfed threshold value, the teratve process termnates and the current transformaton matrx can be determned. Although an teratve technque can be a tme-consumng procedure, because the nertal atttude determnaton algorthms may be repeated for several tmes at all the IMU Fg. Iteratve computaton of rotaton matrces IET Radar Sonar Navg., Vol., No. 1, February 008

5 nodes at each teraton, the advantage of ths method s that the errors of the estmates of the rotaton matrces are ndependent of the dynamc models of the rotaton matrces. Uncertantes n the local atttude matrx estmates can contrbute to errors n the estmaton of the rotaton matrx. However, the nertal atttude determnaton algorthms provde an effectve flter, whch can reduce the effect of IMU measurement nose on the rotaton matrx estmates... Analytcal method: Ths method s based on the development of analytcal dynamc models of the transformaton matrces. If the body frame s used as a reference frame to represent the relatve rotaton moton of the other frames and the measured angular veloctes, (1) () can be rewrtten n terms of angular rates as follows V I= ¼ V I= þ V = () V I= ¼ V I= þ V = (4) V I= ¼ V I= þ V = (5) where V s a skew-symmetrc matrx of the correspondng angular rate vector v. The superscrpt denotes that the angular rate vectors are expressed n terms of the body coordnates. In terms of atttude matrx dfferental equatons, () can be wrtten as Therefore T V I= ¼ V I= ^T T () ^T ¼ (V I= V I= )T () V I= ¼ T V I=T (8) Smlarly, the dfferental equaton of the rotaton matrx s gven by ^T ¼ (V I= V I= )T (9) V I= ¼ T V I= T (0) where V I=, V I= and V I= consst of the local absolute angular rate vectors, estmated from the local IMU measurements at nodes, and. The rotatonal transformaton matrx between and s then computed from the followng equaton T ¼ T T (1) Clearly, the dynamc models of the rotaton matrces are non-lnear matrx dfferental equatons, where the ntal matrces can be derved from the statonary transformatons and these dfferental equatons are solved at each measurement tme. In comparson wth the teratve method, the analytcal method avods the teratve computaton of the nertal atttude determnaton algorthms at all the IMU nodes. However, the IMU measurement nose may affect the accuracy of the solutons of the rotaton matrces because the IMU outputs are drectly used n the rotaton matrx dfferental equatons. Consequently, pre-processng flters may be requred to reduce the measurement nose. If the node IMUs have a good qualty (say better than 0.058/h), the teratve method s especally effectve n determnaton of the rotaton matrces between nodes. The analytcal method s more applcable to dynamc IMU algnment n an nertal network system. 4 Dstrbuted data fuson algorthms 4.1 Kalman flter algorthm The dstrbuted nertal network fuson algorthms developed n ths paper are based on Kalman flterng technques. Consder the dscrete-tme stochastc process where the system and measurement models are gven by x(t k ) ¼ F (t k, t k 1 )x(t k 1 ) þ G(t k 1 )w(t k 1 ) () z(t k ) ¼ D(t k )x(t k ) þ y(t k ) () where F s an n n state transton matrx, D an m n measurement matrx, x(t k ) an n-system state, w(t k ) a q-addtve process nose, whch takes nto account the perturbatons to the system, G(t k )ann q matrx, z(t k )an m-measurement vector and y(t) s an m-addtve measurement nose vector. It s assumed that the nose vectors w(t k ) and y(t) are ndependent, zero-mean, whte Gaussan sequences of covarance Q(t k ) and R(t k ), respectvely. The ntal system state x(t 0 ) s a Gaussan dstrbuted random varable and s ndependent of the nose, wth an ntal value x 0 and covarance P 0. The Kalman flter algorthm s as follows [1]. Step 1: Intalzaton P(t 0 ) ¼ P 0 ; ^x(t 0 ) ¼ x 0 Step : Tme update (effect of dynamcs, predctor) ^x(t k ) ¼ F (t k, t k 1 )ˆx (t þ k 1) P(t k ) ¼ F (t k, t k 1 )P(t þ k 1)F T (t k, t k 1 ) þ G(t k 1 )Q(t k 1 )G T (t k 1 ) r(t k ) ¼ z(t k ) D(t k )^x(t k ) S(t k ) ¼ D(t k )P(t k )D T (t k ) þ R(t k ) Step : Measurement update (effect of measurement, estmator) K(t k ) ¼ P(t k )D T (t k )S 1 (t k ) ^x(t þ k ) ¼ ˆx(t k ) þ K(t k )r(t k ) P(t þ k ) ¼ P(t k ) K(t k )D(t k )P(t k ) From the predctor and the estmator equatons gven above, the Kalman flter outputs valuable statstcal nformaton that can be used to montor both the convergence and the consstency of the flter estmaton procedure. The outputs of the predctor nclude the flter nnovaton r(t k ) and ts covarance S(t k ), whereas the estmator outputs the flter resdual r(t þ k ) and the resdual covarance S(tþ k ), defned as follows r(t þ k ) ¼ z(t k ) D(t k )x(t þ k ) S(t þ k ) ¼ H(t k )P(tþ k )DT (t k ) þ R(t k ) It has been shown that the flter nnovaton and resdual processes are a zero-mean whte Gaussan random sequence IET Radar Sonar Navg., Vol., No. 1, February

6 n normal operaton f the Kalman flter model matches the true system model [1, 14]. Ths feature can be exploted n the analyss of the Kalman flter ntegrty to check the consstency of measurement data for sensor falure detecton and to montor the flter dvergence. 4. Dstrbuted nertal data fuson algorthm Assume that all the local IMUs are ndependent of each other and ther measurements have a Gaussan probablty dstrbuton. Then, the errors of the local nertal state estmates are also a Gaussan dstrbuted random vector and the probablty densty functon of the local nertal state s gven by 1 p(x) ¼ qffffffffffffffffffffffffffffffffffffffffffffff exp 1 (p) (x ^x)t P 1 x (x ^x) (4) det P x where x s a three-dmensonal local nertal state vector, for example, an acceleraton or angular rate vector, and P x s the covarance matrx of the error of the local nertal state estmate. From least-squares estmaton P x ¼ (H T H) 1 H T RH(H T H) 1 ¼ H R[H ] T (5) where H ¼ (H T H) 1 H T s the pseudo-nverse matrx of the IMU desgn matrx H (the superscrpts and subscrpts of the desgn matrx H have been omtted to smplfy the expresson). The obectve of nertal measurement fuson s to generate optmal estmates of all the local nertal states. If the optmsaton crteron s defned as the maxmum of the condtonal probablty P(x^x, ^x, ^x ) then, because all the IMU measurements are ndependent, the condtonal probablty densty functon of the true local nertal state at each IMU node can be represented as follows p(x^x, ^x, ^x ) ¼ p(x) ¼ p(x^x )p(x^x )p(x^x ) () Applyng the maxmum lkelhood estmator to () and ncorporatng (10) (1), the nertal data fuson algorthms at each IMU node can be derved as follows detect and solate nertal sensor falures f devatons of the estmator resduals are used as a test statstc. In effect, the nertal measurement fuson s a pre-processng procedure for the second-stage fuson. 4. State fuson flterng algorthm From () and (), the local dynamc models embedded n each node of the nertal network system can be descrbed as follows x J (t k ) ¼ F J (t k, t k 1 )x J (t k 1 ) þ G J (t k 1 )w J (t k 1 ) (40) z J (t k ) ¼ D J (t k )x J (t k ) þ y J (t k ) (41) where J ¼,, denotes the IMU nodes. The normalsed measurement models of navgaton adng systems, the normalsed SRIMU error dynamc models and the normalsed error dynamc models of nertal systems are gven n [15 1]. These models can be appled to all the IMU nodes f the correspondng coordnate frames are specfed. As dfferent frames are used n the ndvdual nodes, each local dynamc model descrbes ts local states, whch wll be dfferent from the local states represented by the other dynamc models. The local state vector x J can be parttoned nto the local system state x 1J and the local sensor error state x J, that s x J ¼ [ x T 1J x T J ]T (4) The local system states at the nodes are referred to as smlar states and the transformatons between these smlar states are gven by the dynamc transformaton matrces. The measurement vector z J can be decomposed nto three sub-vectors as follows z J ¼ [ z T JL z T JS z T JA ]T (4) where z JL s the measurement vector provded by the local sensor systems, z JS s the measurement vector provded by the navgaton-adng systems and z JA s the combnaton of the nertal measurements assmlated from other IMU nodes. Because these three measurement vectors are ndependent of each other, the followng decompostons can be obtaned or " x J ¼ X # 1 T J l P 1 x,l T X l J l¼,, l¼,, T l J P 1 x,l H l m l, J ¼,, () " x J ¼ X # 1 T J l P 1 x,l T X l J l¼,, l¼,, T l J H T l R 1 l m l (8) D J ¼ [ D T JL D T JS D T JA ]T (44) y J ¼ [ y T JL y T JS y T JA ]T (45) R J ¼ blockdag[ R JL R JS R JA ] (4) The archtecture of the state fuson flter algorthm at each node s llustrated n Fg. where the local Kalman P 1 x, J ¼ X l¼,, T l J P 1 x,l T l J, J ¼,, (9) Equatons () (9) comprse the nertal measurement fuson algorthm at each IMU node. Although nertal measurement fuson s manly used to provde relable and accurate local nertal state estmates, one addtonal beneft of ths approach s that the outputs can be used to 5 Fg. State fuson algorthm archtecture IET Radar Sonar Navg., Vol., No. 1, February 008

7 flter uses the assmlated sensor measurements to estmate the local state. The local state fuson flter combnes the local estmate and the assmlated estmates from the other nodes to update the local estmate. All the local Kalman flters process the three forms of the measurements to obtan the local estmates. Applyng the Kalman flterng algorthm to (40) and (41), and consderng (4), (44) and (4) yelds ^x J (t k ) ¼ F J (t k, t k 1 )x J (t k 1 ) (4) P J (t k ) ¼ F J (t k, t k 1 ) P J (t þ k 1)F T J (t k, t k 1 ) P 1 J P 1 J þ G J (t k 1 )Q J (t k 1 )G T J (t k 1 ) (48) (tk þ ) ¼ P 1 J (tk ) þ D T J (t k )R 1 J (t k )D J (t k ) ¼ P 1 J (t k ) þ X k¼jl,js,ja D T l (t k )R 1 l (t k )D l (t k ) (t þ k )^x J (tþ k ) ¼ P 1 J (t k )^x J (t k ) þ DT J (t k )R 1 J (t k )z J (t k ) ¼ P 1 J (t k )^x J (t k ) þ X l¼jl,js,ja (49) D T l (t k )R 1 l (t k )z l (t k ) (50) where ^x s the state estmated by the local Kalman flter and x s the state updated by the local state fuson flter. To update the locally estmated smlar states at each node usng the smlar state estmates assmlated from the other nodes, a state fuson flter s needed n each node. Defnng a quadratc cost functon at node as follows F ¼ X J¼,, (T Jx J x ) T T JP 1 J T J (T Jx J x ) (51) where x s the true local smlar state at and F s a cost functon used to measure the dsplacement of the local state estmate from ts true value. The state fuson flter s desgned to mnmse F and s referred to as the mnmum weghted mean square error crteron. Dfferentatng F and settng the result to zero yelds P 1 x ¼ P 1 ¼ (P 1 (P 1 ^x þ T P 1 ^x þ T P 1 ^x ) (5) þ T P 1 T þ T P 1 T ) 1 (5) Smlarly, the update equatons of the smlar states at the nodes and are gven as follows P 1 x ¼ P 1 ¼ (P 1 (P 1 ^x þ T P 1 ^x þ T P 1 ^x ) (54) þ T P 1 T þ T P 1 T ) 1 (55) x ¼ P 1 (P 1 ^x þ T P 1 ¼ (P 1 þ T P 1 P 1 T þ T ^x þ T P 1 ^x ) (5) P 1 T ) 1 (5) Equatons (4) (50) and (5) (5) consttute the dstrbuted state fuson flter algorthms for the nertal network system. The state fuson explots the redundances of the smlar system states and consequently, ths fuson method can greatly mprove the fault tolerance of an nertal network system. From (4) and (48), the outputs of each state fuson flter are fed back to the correspondng local Kalman flter. Ths feedback operaton allows the local Kalman flter to accurately estmate and calbrate ts sensor errors. Ths procedure s known as the dynamc transfer algnment of the nertal network system. Both the nertal data fuson and state fuson procedures mprove the estmaton accuracy of the nertal states and smlar states. The local Kalman flters allow all slave node IMUs to be algned n-flght to the master nertal navgaton system. Compared wth the federated flter and other dstrbuted flter archtectures [18, 19], the dstrbuted data fuson algorthms presented n ths paper afford the followng advantages 1. The flter archtecture s relatvely smple.. Each local Kalman flter estmates ts own nertal sensor errors rather than pseudo-sensor errors. Therefore the Table 1: Smulaton parameters of nertal sensors Sensor Parameters gyro drft tme const, s gyro drft err, deg/h gyro bas err, deg/h gyro SF err tme const, s gyro SF error, ppm gyro Az msalgn err, arcsec 1 gyro El msalgn err, arcsec gyro nose, deg/sqrt(h) accel drft tme const, s accel drft err, mg accel bas err, mg accel SF err tme const, s accel SF err, ppm accel Az msalgn err, arcsec 1 1 accel El msalgn err, arcsec accel nose, mg/sqrt(hz) IET Radar Sonar Navg., Vol., No. 1, February 008 5

8 Fg. 4 True poston traectory estmates of the local flters can be used to correct the local IMU errors and the smlar states.. By assmlatng the smlar local states, each local state fuson flter provdes an nherent fault tolerance capablty. 4. The nner feedback from the state fuson flter to the local Kalman flter at each node allows all IMUs n an nertal network system to be automatcally algned to the node IMU frame wthout addtonal algnment algorthms or procedures. As a result, many tradtonal nertal algnment procedures, such as transfer algnment, are elmnated. IMU) at the slave nodes and 0.058/h (a typcal navgatongrade IMU) at the node. All smulated accelerometers at the slave nodes have a typcal bas of 00 mg, whereas the accelerometers at the node have a bas of 100 mg. The SRIMU smulator also models other error sources, ncludng tme-dependent sensor drfts (random drfts), zero offset (bases), msalgnments, scale factor errors, as well as sensor nose. The parameters used n the smulaton are summarsed n Table 1. The smulaton results show that the change of accelerometer bas has no sgnfcant effects on the performance of the master and slave node data fuson flters. Because the man purpose of ths smulaton s to evaluate the estmaton accuracy of the dstrbuted data fuson algorthms, only two nodes (one master and one slave) were used n the smulaton. Addtonal nodes do not change the archtecture of the dstrbuted data fuson flter but sgnfcantly ncrease the complexty of the computaton. The dynamc relatonshp between the master and the slave nodes was smulated by a snusodal functon. In the dynamc models of the two-node nertal network system, the smlar system states at both the node and the slave nodes contan the nne basc navgaton state errors (three lnear poston errors, three velocty errors and three atttude angle errors), whch are represented n the dfferent local frames. The local sensor error state vector at the node contans accelerometer-related error 5 Smulaton results A mult-sensor smulaton envronment, consstng of a GNSS smulator, an SRIMU smulator and a true traectory generator, was developed n Matlab to test and evaluate the fuson algorthms descrbed n ths paper. The GNSS smulator provdes raw GNSS measurements, ncludng pseudoranges and pseudo-range rates at the rate of 1 Hz. The SRIMU smulator can smulate several SRIMU confguratons, ncludng cube, cone and dodecahedron confguratons. The SRIMU smulator, workng wth a true traectory generator, generates realstc SRIMU measurements at an update rate of 50 Hz. The true traectory s used as a reference to examne the accuracy of the estmated arcraft moton states. The smulated gyro sensors have gyro drft rates of 108/h (a typcal tactcal-grade Fg Arcraft horzontal manoeuvres Fg. Arcraft vertcal ptch manoeuvres a Vertcal velocty manoeuvres b Ptch manoeuvres IET Radar Sonar Navg., Vol., No. 1, February 008

9 Fg. 8 Error standard devatons at the master node wth a gyro drft rate of 0.058/h a Atttude error standard devatons at node b Velocty error standard devatons at node c Poston error standard devatons at node Fg. Smlar state errors at the master node wth a gyro drft rate of 0.058/h a Atttude errors at node b Horzontal velocty errors at node c Horzontal poston errors at node terms (bas, tme-related drft error, msalgnments and scale factor error), gyro-related error terms (random constant bas, tme-related drft error, msalgnments and scale factor error), GNSS recever clock errors (phase and frequency errors), magnetc headng error and ar data system error (ar pressure alttude and ar speed errors), as specfed n [1] and redefned n the followng equatons. All navgaton adng sensors are related to the node. The error state and measurement vectors at the node are as follows x ¼ x basc state errors x sensor errors GPS pseudorange vector GPS pseudorange rate vector z ¼ magnetc headng output 4 ar pressure alttude and ar speed 5 SRIMU resdual vector IET Radar Sonar Navg., Vol., No. 1, February

10 x basc state errors ¼ 4 x 1 x x x 4 x 5 x x x 8 dw R dl R dh dv 1 x ¼ dv 1 y dv 1 x 5 y 5 x sensor errors ¼ 4 x z Rx Clk error states SRIMU accel error states SRIMU gyro error states Magnetc headng error state Barometer bas state True ar speed bas state x clk pha x clk rate x accel 1. x accel n ¼ x gyro 1.. x gyro n x Mag 4 x 5 Bar x ADS The local sensor error state vector at the slave node contans only local nertal sensor errors; the measurement vector at the slave node s defned as follows poston resdual vector velocty resdaul vector z ¼ 4 atttude resdal vector 5 SRIMU resdual vector The smulated arcraft can perform arbtrary manoeuvres up to a maxmum acceleraton of.0 g. A typcal flght traectory used n ths study s shown n Fg. 4 n whch the arcraft fles from the start pont to the north-east, then completes three sets of 908 rght turns, returnng to the start pont. Fgs. 5 and show the traectores of the horzontal and vertcal manoeuvres, respectvely. The smulaton study focused on the assessment of the performance of the dstrbuted data fuson algorthms n terms of estmaton accuracy and convergence. Tme delay problems n an nertal network system were not consdered although they may cause problems n nertal network algnment procedures. In the followng case study, both the master and slave nodes use SRIMUs. The smulated IMU at the master node s a 5-sensor cone SRIMU wth a gyro drft rate of 0.058/h. The smulated IMU at the slave node s also a 5-sensor cone SRIMU but wth a gyro drft rate of 108/h. The smulaton results of the smlar system states derved by the master node fuson flter are shown n Fgs. and 8 where the gyros have a drft rate of 0.058/h and GPS provdes contnuous pseudorange and pseudo-range rate observables at the rate of 1 Hz. Fg. shows the absolute errors of the smlar system states n comparson wth the true traectory parameters and Fg. 8 shows the error standard 0 5 Fg. 9 Smlar state errors at the slave node wth a gyro drft rate of 108/h a Atttude errors at slave node b Horzontal velocty errors at slave node devatons of the smlar system states estmated by the master node fuson flter. After the ntal algnment (5 mn), the absolute atttude errors (the dfference between the true and estmated values) mostly remaned wthn 0.18 durng a smulated h flght n Fg. a. The lateral velocty errors were,0.5 m/s wth a probablty of 98% n Fg. b and the accuracy of the lateral velocty estmates s better than 0. m/s(s) nfg. 8b. As expected, the vertcal velocty has a larger error compared wth the lateral velocty error owng to the vertcal dluton of precson of GPS. The node data fuson flter quckly converges to the steady state and acheves steady-state atttude estmates, 0.18 (1s) n Fg. 8a. The accuracy of the yaw estmate s slghtly less than other atttude angles because of a larger ntal yaw angle. The postonng accuracy of the master node fuson flter depends on the GPS postonng accuracy. The ump changes of the standard devaton of the poston and velocty error states n Fgs. 8b and c were caused by the changes of the GPS satellte geometres. For the slave node wth a gyro drft rate of 108/h, Fg. 9 shows the absolute errors of the local smlar states and Fg. 10 shows the correspondng standard devatons of the local smlar states. After the ntal algnment (10 mn), the accuracy of the atttude estmates at the slave node shows no sgnfcant degradaton although the convergent rate of the slave node data fuson flter s slower than the master node fuson flter n Fg. 9a. The accuracy of the atttude estmate derved by the slave IET Radar Sonar Navg., Vol., No. 1, February 008

11 algnment of the slave IMU (10 mn), the accuracy of the estmated ptch rate at the slave node s sgnfcantly better than the drect measurements at the slave IMU. These smulatons show that the dstrbuted nertal network algorthms presented n ths paper can be used to acheve accurate navgaton states where the local nertal state vector s derved for low-cost IMUs. It can be seen that the local Kalman flter and the state fuson flter combne to reduce sgnfcantly the hgh-frequency ptch rate nose. Conclusons Fg. 10 Error standard devatons at the slave node wth a gyro drft rate of 108/h a Atttude errors standard devatons at slave node b Velocty errors standard devatons at slave node fuson flter s better than 0.8 (s) nfg. 10a and the accuracy of the lateral velocty derved by the slave fuson flter s better than 0.8 m/s (s) n Fg. 10b. From Fgs. 9a and 10b, the estmates of the local smlar states change slghtly durng hghly dynamc manoeuvres. The measured rotaton rate of the local ptch axs at the slave node and the correspondng estmate gven by the slave fuson flter are shown n Fg. 11. After the ntal The paper revews recent developments n arcraft nertal network systems, wth partcular emphass on skew redundant IMUs. The overall system archtecture and navgaton reference frames are developed for dstrbuted nertal network nodes. Two methods are presented to compute the rotaton matrces for the dynamc nertal sensng model for both rgd body arcraft and flexble arframe confguratons. Dstrbuted data fuson algorthms have been developed for nertal network systems, ncludng the development of dstrbuted state fuson flters and nertal measurement fuson algorthms. These algorthms are applcable to multple IMU network systems n arcraft wth a flexble structure. Extensve smulaton studes were undertaken combnng an SRIMU smulator wth a GNSS smulator. Specfc manoeuvres were flown so that the sensor derved measurements and arcraft moton states could be compared drectly wth the actual arcraft traectory. Smulaton studes have demonstrated the feasblty of dstrbuted data fuson algorthms for local nertal state estmate and dynamc nertal network algnment. A wde range of low-cost nertal sensors were evaluated, coverng the performance of the maorty of currently avalable nertal sensors. The smulaton results presented n the paper show that low-qualty IMUs (gyro drft rate up to 08/h) can be precsely algned to a hgh-qualty IMU (gyro drft rate of 0.058/h) wth an algnment accuracy better than 0.8/h by usng the dstrbuted data fuson flters and that the local fuson flter can accurately estmate the local nertal states. The algorthms presented n ths paper provde redundant nertal nformaton n a form that s applcable to current falure detecton and solaton methods used to detect sensor falures. References Fg. 11 Rotaton rates of local ptch axs at slave node wth gyro drft rate of 108/h 1 Kelley, R.T., Carlson, N.A., and Bernng, S.: Integrated nertal network. Proc. IEEE PLANS, 1994, pp Bernng, S., Howe, P., and Jenkns, T.: Theater-wde reference nformaton management. Proc. IEEE NAECON, 199, pp Kaser, J., Beck, G., and Bernng, S.: Vtal advanced nertal network. Proc. IEEE PLANS, 1998, pp Harrs, R.L.: Modular avoncs: ts mpacts on communcaton, navgaton, and dentfcaton (CNI). Proc. IEEE NAECON, 1988, pp Swanson, D.L.: Evolvng avoncs systems from federated to dstrbuted archtectures. Proc. IEEE/AIAA/NASA 1th Dgtal Avoncs Systems Conf., 1998, pp. D/1 D/8 Schmdt, G.T.: GPS/INS technology trends for mltary systems. Draper Technology Dgest, 1998, pp. 1 avalable at: Barbour, N. Schmdt, G.T.: Inertal Sensor Technology Trends, IEEE Sensors J., 1, (4)Dec 001, pp. 9 8 Glmore, J.P., and Mckern, R.A.: A redundant strapdown nertal reference unt (SIRU), J. Spacecraft Rockets, 19, 9, (1), pp Allerton, D.J., and Ja, H.: An error compensaton method for skewed redundant nertal confguratons. Proc. ION 58th Annual Meetng/ GIGTF 1st Gudance Test Symp., Albuquerque, USA, 00, pp IET Radar Sonar Navg., Vol., No. 1, February 008 1

12 10 Daly, K.C., Ga, E., and Harrson, J.V.: Generalzed lkelhood rato test for FDI n redundant sensor confguratons, J. Gud. Control, 199,, (1), pp Brown, A., and Sturza, M.A.: The effect of geometry on ntegrty montorng performance. Proc. ION 4th Annual Meetng, 8 June Carson, N., Kelley, R., and Bernng, S.: Dfferental nertal flter for dynamc sensor algnment. Proc. ION Natonal Techncal Meetng, 1994, pp Maybeck, P.S.: Stochastc models, estmaton, and control (Academc Press, 199), vol Hanlon, P.D., and Maybeck, P.S.: Multple-model adaptve estmaton usng a resdual correlaton kalman flter bank, IEEE Trans. Aerosp. Electron. Syst., 000,, (), pp Roger, R.M.: Appled mathematcs n ntegrated navgaton systems (AIAA, Inc., 000) 1 Allerton, D.J., and Ja, H.: Redundant mult-mode flter for a navgaton system, IEEE Trans. Aerosp. Electron. Syst., 00, 4,(1), pp Ja, H.: Data fuson methodologes for multsensor arcraft navgaton systems, PhD thess, Cranfeld Unversty, Bedfordshre, UK, Carlson, N.A.: Federated flter for dstrbuted navgaton and trackng applcatons. Proc. ION 58th Annual Meetng/GIGTF 1st Gudance Test Symp., Albuquerque, USA, 00, pp Allerton, D.J., and Ja, H.: A Revew of multsensor fuson methodologes for arcraft navgaton systems, J. Navg., 005, 58, (), pp IET Radar Sonar Navg., Vol., No. 1, February 008