In this paper factorization filtering algorithms are described and used for processing13; data from a typical flight test range. Specifically U-D (unit upper triangular-diagonal)13; factorization based Kalman filtering algorithms are considered. The algorithms are13; validated using simulated data and implemented in MATLAB and alpha DEC machines.13; UDP protocols are used to transfer data from one DEC machine to another where the13; UD filter algorithm is activated to process the data. A very brief description of the fusion13; scheme in which the UD filtering algorithms are being used is given

Girija, G

Raol, JR

Shantha Kumar, N

National Aerospace Laboratories Institutional Repository

Factorization Filtering Algorithm with Colored Noise for Tracking N. Shanthakumar, Girija G and Raol J.R. Scientists, System I ation Laboratory Flight Mechan ntrol Division National Aerospace La es-Bangalore-5&0017 e-rnail: jrraol In this paper factorization filtering algorithms are described and used for processing data from a typical flight test range. Specifically U-D (unit upper triangular-@iagonal) factorization based Kalman filtering algorithms are considered. The algorithms are validated using simulated data and implemented in MATLAB and alpha DEC machines. UDP protocols are used to transfer data from one DEC machine to another where the UD filter algorithm is activated to process the data. A very brief description of the fusion scheme in which the UD filtering algorithms are being usEd is given. values of thdse parameters which completely specify the based on the measurements (11 These electro-optical devices (EOD), sonar etc The meGurements are processed using an optimal estimation algorithm. 'A precision track &termination estimator takes the measurement noise into account and ermines the track that provides a "best fit" to the collected data. Major constituents a track determination process are : vehicle's (system) kinematic motion model, asurements and estimation techniques. For a flight test range the tracking of a flight vehicle and sensor fusion are of great importance. In this paper, UD-factorization filtering algorithm, which directly handles bias parameters and correlated process noise in addition to the state estimates, is described. The algorithms are implemented in PC MATLAB and'C language (in alpha DEC computer). The algorithms are validated using simulated and real data. Using socket-programming features the UDP protocol for data communication has also been established between two alpha DEC computers. 2. Tracking Filter Kalman filter has found very wide application in tracking problems because of its optimal and recursive features. The conventional Kalman filter algorithm is not numerically robust due to round-off errors, and numerical accuracy can degrade to the point where results cease to be meaningful. Typical problems that could occur are loss of positive- definiteness of the covariance matrix resulting from numerical errors such as finite word length computations and cancellation errors due to the subtraction term in the Kalman covariance update. Square-root filter formulations offer a solution to this problem of numerical accuracy and hence stability of the filter. The improved numerical behavior of square root algorithms is due to a reduction of the numerical ranges of the variables. In the UD filter [2 ] ,  the covariance update formulae and the estimation recursions are - -- - . -  RADAR SYMPOSIUM INDIA - '99 371 . reformulated so that the covariance matrix does not appear explicitly. Specifically recursions are used for U and D factors of the covariance matrix. Computing and updating with triangular and diagonal matrices involve fewer arithmetic operations. The algorithm processes measurements, one component at a time. 2.1 Mathematical Model A mathematical model of the vehicle state includes position, velocity and sometimes acceleration as state variables. The measurement model relates state variables to available measurement variables. For simplicity the states of motion are defined in the spherical coordinates such that the state equation can be decoupled into three independent channels. Then the tracking filter can work independently on each channel. The problem addressed in this paper is represented by the following set of equations: Measurement Model: zi = H x ,  +v, ( 2 )  Here. x is the state vector, w is the process noise with zero mean and covariance matrix Q. z is the measurement vector and v is the measurement noise with zero mean and covariance matrix R. all of appropriate dimensions. 2.2 U-D factorization Kalman filter The tracking filter is implemented in the factorized form. Time Propaqation Alqorithm State vector evolution (prediction) Covariance update . .Y,.; = @  . x .  I.. i r,-, = @Pj& +(;oc;' - . ^ .  With P = lN)llr and Q, the time update factors 6 and 5 are obtained through modified Gram-Schmidt orthogonalization process [2] .  The matrix U is an upper triangular matrix with unit elements on its main diagonal and D is a diagonal matrix. covariance matrix P. are a technique for implementing "square root filtering" without requiring cbmputation of square roots. The U-D Kalman filtering algorithm is considered efficient, stable and accurate for real-time applications "4. \ Covariance and gain processing algorithms. operating on U and D factors of state error i -- The ll,l) factors of P = WDWT may be computed now. For j= n, n-l,.. ., 2, the following equations are recursively evaluated as shown below: The measurement update in Kalman filtering combines a priori estimate X and error covariance Fwith scalar observation I = a T x + v ;  a7 = H  to construct an updated (filtered state) estimate and covariance as follows: K = pula, i = X + K(z-aT?),  a = a -  Pa+r  7 -  i, = I; - K n j  Here, r is the measurement noise variance (for scalar data processing). Gain K and updated covariance factors fi and 6 can be obtained from the following equations: j = ITTLI ;  f = (.f 1 ,..,) f") 1,. = u'. I ,  f.= ZIr /a , ;  a, = r +.,A; K: = (v, o...o) - - 1' = U f :  i=l.2, ...! n For j=2, ..., n recursively the following equations are evaluated: a, = aj_, +vJ . f j  d .  = d,ui.: iu.: - - i, = 1 4 ,  + i , R , :  : ,  RJ = -{: !uJ..l (7 )  - - Where I1  =[GI ,..., ITn], fi = [ j l , ,  .... G ! , ]  and the gain is given by K = K,,,, la,, . Here d =is predicted diagonal element, and j i  is the updated diagonal element of the D matrix. - 1 - -  7 - E d  RADAR SYMPOSIUM INDIA - '99 . 2.3 UD Filter for Correlated Process Noise and Bias Parameters A mathematicat model representing a trajectory is usually not exact (even in a statistical sense) and the process noise is hardly a strictly white noise process. For ihis case the state model is given as: .,] 0 (9) Here x is the state vector, p is the (state) variable representing the correlated noise and y is the bias vector. The transition matrix is almost triangular. The mapping (time propagation) of the U-D factors is carried out using the following equivalence [2]: A priori factors: ; b=diagonaI(6x ,iP ,by)  all at time j u,=v&" +v,fip ; i7w=vr~,+vp~w+vyljy u =Q = f i  ; E  = D = 6  u, =MU, = MU, ; u, = u, - - _  U,DJJJ = (V, fix)bx (V*fi1)' - Y Y  Y Y Y  Y a - - - - (1 0) The above updating is mechanized by using the modified Gram-Schmidt orthogonalization algorithm. The factors related to correlated process noise and the bias are mapped as final factors using the modified Gram-Schmidt algorithm to get: -- - UDU' = W Diagonai(D,Q)WT i 3. Sensor Fusion Scheme [3] In order to be able to. use sensor-channels such as EOT, PCMC, S-Band. two TM, RADAR 1, RADAR 2, INS, and GPS for fusion, it is necessary to develop fusion logic to use the information from these sensors, Fig. 1. The priority logic is  decided based on the sensor accuracy within the range of the sensor capability. Based on this priority logic the following sequence could be given to the sensors (of the first module) within the range limit of, say RL km. EOT PCMC TM and S-Band fusion S-Band TM Track loss f For range more than RL km PCMC radar tracks the vehicle. For the second module which contains the RADARS, the following sequence is fdlowed RADAR 1 and dADAR 2 fusion RADAR1 RADAR2 Track loss . For the third module which contains the INS and GPS, the following sequence IS followed INS and GPS fusion (GPS data replaced by INS) INS GPS (GPS data replaced by INS) Track loss The U-D filters described in the previous section are being used in the above sensor fusion scheme for tracking of a vehicle and data fusion. 4. Results and Discussions Simulated data with correlated process noise have been generated. The transition matrices used are given by: vi =[::I ;M, =exp(-Arlr) ; r=Orom (1 1) The states are position and velocity. A constant bias has been added to the position data so that Vy=l. At is the sampling interval equal to 0.5 sec and the value of M is 0.9, indicating high correlation. The UDF (without accounting for correlated noise and bias) and the UDFCPNB (accounting for correlated process noise and bias) are used to process these data using vehicle position only as the observable. Fig. 2 shows the 3 3  RADAR SYMPOSIUM INDLA - '99 comparison of the state estimates obtained by these filters. From the results of state errors (difference of true and estimated state), it is seen that the UDFCPNB performs better than UDF when the data is contaminated by cotrelated noise. This is also supported by the results of Table 1. The real data of a flight vehicle (in Cartesian coordinates X,Y,Z) are processed using both the UD filters. The transition matrix of the state eqn. (9) is given as: 1 A i  A i l 1 2  v r = k  ; y (12) and H=[l  0 01. State vector x has position, velocity, and acceleration as its components. The figs. 3 and 4 show the results of this analysis. From fig. 3 it is seen that the performance of both the filters is grossly similar. However, from fig. 4 and Table 2 it can be inferred that the performance of UDFCPNB is better than the UDF. 5. Concluding Remarks UDF UDFCPNB Mean Position velocity 0.8489 1.68 -8.4 -17.21 1 Table 2: Results of Real Data- Mean values of residuals* Mean of Residuals 1 TMs EOTs SB PCMCs Radar’ Radar2 INS GPS (lRAJ2) I MPC3 PC-3 MPCl PC-1 Raw Data f +. .+ UDF (Intermediate Storage) -4 RAW DATA CONVE RS 10 r\r SNV (& Fixed- MSDF BLOCK t DEC ALPHA server PC- DSS UD F U-D Filter PC Pesonal Computer UDFF U-D Fusion Filter TM Telemetry MSDF Multi Sensor Data Fusion SBR Side Band Radar ISB Intelligent Switch Board DSS Decision Support System Fig. 1 Block diagram of data acquisition and trackinglfusion .. RADAR SYMPOSIUM INDIA - ‘99 3 5  - 0x10 5 0 100 200 300 400 500 0 100 200 300 400 500 20 al 0 S .- n o  -20 0 100 200 300 400 500 15 '7 I ESTIMATED 0 100 200 300 400 500 0 -20 L L (u al > - -40 -3a 0 100 200 300 400 500 Time (Secs) I I 00 30 400 500 O loo *me (~ecs' j  Fig. 2 State Estimates, State error - Simulatedjdata ~ A R  s ~ P o S ~ U M  I N D ~ A  - '99 @? 400 200 0 h E - -200 2 07 x UDFCPNB -4001 , , , 1 1 -600 I 0 50 100 150 200 1000 500 h 0 E v v) a N L -500 -1 000 2oo/ -600 iooa 800 600 h E n Y W u) u) LT 400 200 100 150 200 0 50 0 50 100 150 200 0 50 100 150 200 Time ( s e c s )  Time ( secs )  Fig. 3 Residuals and RSSPE - Real Data 4- RADAR SYMPOSIUM INDIA - ‘99 1 3ik4 20 I I I I I I I I I .- m X ....... ........... . ~,," : :  . .  . .  . .  . .  . .  . .  . .  . .  . .  UDF ii -20 L I I I I I I I I I 0 20 40 60 80 100 120 140 160 180 200 - " - - - :; tn i f  >. m .- 3 m -20 I I I I I I I I I I I I I I I I I I 0 -  - i ;". . j : ............. . .  i , !  -20 ; j  . .  - .. - .. , -40 I I I I I I I I I 6 E m - lA m .- N Fig. 4. Bias Estimates - Real data 

Factorization filtering algorithm with colored noise for tracking

NAL-IR

http://nal-ir.nal.res.in/4574/1/Factorization_filtering__Algorithm.PDF

Factorization filtering algorithm with colored noise for tracking

Abstract

Similar works

Full text

Available Versions

National Aerospace Laboratories Institutional Repository

NAL-IR