Track Initialization from Incomplete Measurements

Transcription

1 Track Initialiation from Incomplete Measurements Christian R. Berger, Martina Daun an Wolfgang Koch Department of Electrical an Computer Engineering, University of Connecticut, Storrs, Connecticut 6269, USA Sensor Networks an Data Fusion Group, FKIE, FGAN e.v., 3343 Wachtberg, Germany Abstract Target tracking from incomplete measurements of istinct sensors in a sensor network is a task of ata fusion, present in a lot of applications. Difficulties in tracking using Extene Kalman filters lea to unstable behavior, mainly cause by ifficult initialiation. Instea of using numerical batchestimators, we offer an analytical approach to initialie the filter from a minimum number of observations. Aitionally, we provie the possibility to estimate only sub-sets of parameters, an to reliably moel resulting ae uncertainties by the covariance matrix. The approach will be stuie in two practical examples: 3D track initialiation using bearings-only measurements an using slant-range an aimuth only. Numerical results will inclue performance an consistency analysis via Monte-Carlo simulations an comparison to the Cramer-Rao lower boun. I. INTRODUCTION Target tracking from incomplete measurements, like bearings only or range only, is a topic which has been investigate thoroughly, e.g., target motion analysis an relate questions of observability [1]. The results utilie erivatives of stanar Extene Kalman filters, typical of tracking targets using measurements in polar or spherical coorinates, while moeling their movement in Cartesian coorinates [2], [3]. Especially in the case of incomplete measurements, the initialiation of the extene Kalman filter with an initial state estimate an corresponing covariance is crucial for its performance, otherwise the filter can easily become unstable. In the case of incomplete measurements this can not be accomplishe by irect inversion of the measurement function. Multiple measurements will have to be combine for initialiation, which calls for a sensible ata fusion. Typically, numerical batch estimators are use to fin a Maximum Likelihoo ML estimate [4]. Although these estimators offer close to optimal performance, in the sense of achieving the Cramer-Rao lower boun CRLB in estimation accuracy, they nee a large number of measurements for benign numerical behavior. We offer instea an analytical approach using a minimum number of measurements, to return an initial estimate an a corresponing covariance. By making statistical assumptions about some components of the state vector, we can initialie these state elements with their mean an covariance, an thereby fin an initial estimate even in cases when observability of the full state vector is not given. When splitting the state vector into two parts, one which is estimate an the secon which is initialie through statistical assumptions, the covariance of the latter is a esign choice, but we will still have to erive the cross-correlation between the two. More importantly, the ae uncertainty in the covariance of the estimate parameters will also be moele. This is cause by the loss of information ue to not estimating part of the state vector an instea interpreting them as aitional perturbation. After eriving a general approach, we will apply this to ifferent scenarios, mostly using incomplete spherical measurements typical for raar/sonar applications. The focus will be on position initialiation from bearings-only measurements Sect. III, which was also use in [3], from which this work was strongly inspire by. As an aitional application scenario we will also present: position initialiation from two slant-range an aimuth measurements. Then, both scenarios are numerically evaluate via Monte-Carlo simulation. Focus will be on absolute performance like estimation error an comparisons to the corresponing CRLBs. Consistency of our initialiation will be scrutinie to check if our estimators are unbiase an if the covariance precisely characteries the estimation error. This work has the following structure: After this introuction, we will escribe the system moel an erive our initialiation scheme in Section II. As mentione before Section III will cover the bearings-only scenario, which is followe by the scenario of range an aimuth Sect. IV. We will iscuss our numerical simulations in Section V an Section VI conclues this paper. II. DESCRIPTION OF SYSTEM MODEL AND MAP ESTIMATION A. System Moel Let x be the state vector of the target with imension η x, which is moele in Cartesian coorinates using, e.g., a secon orer motion moel xn +1=F nxn+νn 1 ṙ I tn+1 t x = F n = n r I with x being the state vector, containing position vector r an velocity vector ṙ; ν is the process noise an F the state

2 propagation matrix, all of imension η x. The measurement function h is generally non-linear, epenant on the observer position x s an not invertible, n =hxn x s n + wn. 2 The ero mean, Gaussian measurement noise w with covariance R an the observation ɛmm is the space of the measurements are of imension η. We partition the state vector into two sub-vectors x o an x o, one which is initially estimate by a function of the first measurements an the other which is initialie by appropriate moeling assumptions. W.l.o.g, we can choose to reorer the state vector to achieve, [ ] xo x = 3 x o Then we formally express x as [ x = Kx o + K I x o with K = ], K = [ I ]. 4 Using k ifferent measurements we try to fin a function t with t : M k R ηxo which fulfills the following conition: if x o = E[ x o ] then t hx =x o, i.e., if our assumptions on x o hol, then t gives the correct value x o. In the case of linear functions, this woul be equivalent to an unbiase estimator in terms of the aitional perturbation x o. We have to combine at least k measurements so that kη η xo. To simplify notation, we will use the notation t also when referring to t k an use hx even for mapping to k. Since the function t is generally not reaily available, efining it in a sensible way will be one of the main tasks of this work. Generally if we have kη >η xo, there is no solution to = hkx o + K x o ue to the measurement errors. Instea of picking an x o which reprouces all measurements as close as possible in the ML sense, we rop n arbitrary parts of the measurement to achieve kη n = η xo. Even though we are face with non-linear equations, this guarantees that there will be either none or countable many solutions. By systematically ropping ifferent parts of the measurements we prouce several, kη n, estimates an pick one using an optimality criterion, e.g., the trace of the estimate covariance matrix. If one combination of measurements reners no solution, it is exclue a solution to the case of ambiguity will be given in one example Sect. III-C. Afterwars each of the n remaining atum is merge using Kalman filtering which, if the problem woul be purely linear/gaussian, woul return the same final result for any combination of measurements. Since our problem is inherently non-linear, this way we can initialie from the measurements with the best geometry. B. MAP Estimate using Extene Kalman Filter Lineariation Let the likelihoo function p x be given by a normal istribution N ; hx, R, with a known measurement covariance matrix R. The probability ensity of t given x can be approximate by lineariing t: pt x =N t,t[hx], t R t. 6 Looking at, we can easily see that by efinition t h can be linearie aroun E[ x o ] to t h t hx =x o + K x o E[ x o ] x 7 = x o + G x o E[ x o ] accoringly, we have pt x =N t; x o + G x o E[ x o ], t R t 8 an ue to lineariation we can switch t an x o, px o, x o =N x o ; t G x o E[ x o ], t R t 9 where conitioning on is the same as conitioning on t. If we now substitute x = Kx o + K x o E[x, x o ]=Kt KG x o E[ x o ] + K x o =Kt+KGE[ x o ]+ K KG x o 1 Cov[x, x o ]=K t R t K 11 we can approximate x as Gaussian with the above parameters as 12. To get ri of the conitioning on x o we go along the lines of Bayes total probability theorem for continuous ranom variables, averaging over x o we get, px = px, x o p x o x o. 13 Changing the conitioning an integrating the final pf is given in 14, which results in the MAP maximum a priori estimator of x ˆx = E[x ] =Kt+ KE[ x o ] 1 with covariance P = Cov[x ] = K t R t K + K KGP K 16 KG. px, x o =N x; Kt+KGE[ x o ]+ K KG x o,k t R t K px =N x; Kt+ KE[ x o ],K t R t K + K KGP K KG 12 14

3 C. MAP Estimate using the Unscente Transform To calculate px in 14 we can replace the use of lineariation typical for the Extene Kalman filter, by using the Unscente Transform []. What we basically i to calculate px in the previous case, was to linearie the functional relationships between, x o an x o which we ha in t an t hx. Instea we can irectly erive a functional relationship of the measurements, the parameters x o which are initialie by moeling assumptions an x o the estimate parameters. Using = hx o, x o +w we will solve for x o, i.e., x o = g w, x o, where the function g is in irect relationship to t, since g,e[ x o ] = t. 17 ϑ 1 ϕ 1 2 Fig. 1. r 1 Z x 1 * 2 r 2 ϑ 2 ϕ 2 * x 2 Bearings-only measurements scenario Y X Once we have this, usually non-linear, functional relationship, we can use the Unscente Transform to erive px o from p w, x o, using the functional relationship in 17 to map the influence of the measurement noise an the parameters moele as ranom on the estimate of the other parameters. To fin the probabilty ensity of p w, x o we can use that p w has obviously mean an covariance R an p x o, w is by moelling assumption N x o ;,P, since these parameters are assume to be inepenent of the measurements. Due to conitional inepenency we get p w, x o =p w p x o, w 18 which we can transform into px, using x = Kg w, x o + K x o. 19 D. Calculating the CRLB Using Prior Information Since we consier non-linear measurements an aitive white Gaussian noise, the Cramer-Rao lower boun CRLB can be erive in a stanar way. The general calculation of the Fisher information matrix J as in [2], can be replace by the more specialie formula, J = E { [ x log Λx] [ x log Λx] } = h h 2 Covw 1, x x where Λx =p x is the likelihoo function. Poorly, for a minimum number of measurements, the matrix J will usually not be invertible. This reflects that we can not estimate the full state vector x without aitional assumptions. As information is aitive, these aitional assumptions, in the form of a prior istribution on x o, can be ae to the Fisher information matrix [6], J = J + J P 21 where J P is the Fisher information of the prior. Assuming a Gaussian prior on x o, this will take the following form, [ ] J P = P III. TRACK INITIALIZATION FROM BEARINGS-ONLY MEASUREMENTS A. Scenario Description In this scenario only the spherical coorinates aimuth an elevation are measure, see Fig.1. Initialiing from k measurements is possible, if the measurements are taken at ifferent positions x s n [1], either from ifferent sensors or a moving observer. For brevity without explicit epenency on n, x =[x, y,, ẋ, ẏ, ż] an =[φ, θ], the measurement equations are the following, y ys φ =arctan 23 x x s s θ =arctan 24 x xs 2 +y y s 2 To initialie the position only x o =[x, y, ], it is sufficient to have two measurements k =2, which gives us kη =4 η xo =3. 2 Those two measurements shoul be taken from two istinct arbitrary points x s,i, i =1, 2 with istance x s,1 x s,2 =. 26 W.l.o.g., we can assume them to be on the x-axis at /2 an /2. Using hxn +1 x s n +1=hF nxn x s n an efining T = t n+1 t n as the time ifference between the measurements, the equations can be expresse as, y φ 1 =arctan x + /2 28 θ 1 =arctan x + /22 +y 2 y +ẏt φ 2 =arctan x +ẋt /2 29 +żt θ 2 =arctan x +ẋt /22 +y +ẏt 2

4 epening only on xn an assuming constant velocity. In the following we will refer to w =[φ 1,θ 1,φ 2,θ 2 ] as the stacke vector of the true measurements to simplify notation. Since we will not estimate the velocity, accoringly x o = [ẋ, ẏ, ż], we will have to make statistical assumptions about it. The velocity will be assume to be ero-mean Gaussian istribute N x o ;,P, which is reasonable since there is no preferre irection. Sincewehavekη =4>η xo =3, we are left with one egree of freeom n =1. Accoringly we can choose any three elements of [φ 1,θ 1,φ 2,θ 2 ] which will give us four ifferent inverse functions. Now if we solve 2 for x o,we get x o = t w+f x o, w 3 where we choose f such that f x o, w =for x o = E[ x o ]. This approach will be consequently applie to all four possible combinations of three measurements. B. Two Aimuths an One Elevation Using {φ 1,φ 2,θ 1 } of an solving for x o we first solve for, x = ẋt tan φ 2 ẏt /2tan φ 1 +tanφ 2 an tan φ 1 tan φ 2 = sin φ 1 cos φ 2 sinφ 2 φ T cos φ ẋ sin φ 2 ẏ cos φ 2 1 sinφ 2 φ 1 y = ẋt tanφ 1 tan φ 2 ẏttanφ 1 tan φ 1 tan φ 2 = sin φ 1 sin φ 2 sinφ 2 φ 1 T sin φ 1 ẋ sin φ 2 ẏ cos φ 2. sinφ 2 φ From which we now calculate as, =tanθ 1 x + /2 2 + y 2 =tanθ 1 sin φ 2 sinφ 2 φ 1 T ẋ sin φ 2 ẏ cos φ 2 sinφ 2 φ The first inverse function t 1 w is accoringly t 1 w = sin φ1 cos φ2 sinφ + 2 φ 1 2 sin φ1 sin φ2 tanθ 1 sinφ 2 φ 1 sin φ 2 sinφ 2 φ 1 34 where we use φ 1, θ 1 an φ 2 to estimate x o n an x as Kt 1 w+ KE[ x o ]. To calculate the covariance matrix accoring to 16 we nee the lineariation t1 t1 h w an x, which can be foun in App. I. Alternatively we can use the Unscente Transform UT for which we nee to fin a function g which maps from x o an w to x o. This function g 1 is alreay available in 31-33, i.e. x o = g 1 w, x o. So we are irectly able to use the UT. The estimation function t 2 for {φ 1,φ 2,θ 2 } can be calculate by changing φ 1 φ 2, θ 1 θ 2 an ue to the symmetry of the scenario. For the covariance we also substitute T T. C. One Aimuth an Two Elevations As the secon possibility to calculate x o n, we can use {φ 1,θ 1,θ 2 } of Substituting see Fig. 1 x =ρ 1 cos φ 1 /2 3 y =ρ 1 sin φ 1 36 =ρ 1 37 with the groun range from sensor one ρ 1,weget ρ 1 +żt 2 = tan 2 θ 2 ρ1 cos φ 1 +ẋt 2 +ρ 1 sin φ 1 +ẏt 2 38 This is a quaratic equation in ρ 1 an using q = simpler notation, tan θ2 ρ 2 11 q 2 +2ρ 1 cot θ 1 żt +q 2 cos φ 1 ẋt sin φ 1 ẏt +cot 2 θ 1 ż 2 T 2 q 2 ẋt 2 +ẏ 2 T 2 =. 39 Setting T = equivalent to x o = E[ x o ] = we can calculate the inverse function by solving first for ρ 1 For q =1we get for ρ 2 11 q 2 +2ρ 1 q 2 cos φ 1 q 2 2 = 4 ρ 1 = 2cosφ 1 41 an else ρ 1 = q2 cos φ 1 1 q 2 ± q 1 q 2 1 q 2 sin 2 φ 1 42 To solve this ambiguity we use the secon aimuth φ 2.For geometrical reasons, if q 2 < 1, there is only one positive solution. Otherwise, if φ 2 < π/2 we choose the positive root an negative else. Again, the lineariations neee to calculate the covariance can be foun in App. I To calculate the covariance matrix for t 3 using the Unscente Transform UT, we have to solve 39 for ρ 1, without setting T =. This still means solving a quaratic equation, but is notational more cumbersome ρ 2 11 q 2 +2ρ 1 cot θ 1 żt +q 2 cos φ 1 ẋt sin φ 1 ẏt +cot 2 θ 1 ż 2 T 2 q 2 ẋt 2 +ẏ 2 T 2 =. 43 With Q = cot θ1żt+q2 cos φ 1 ẋt sin φ 1ẏT 1 q 2, we calculate ρ 1 = Q ± Q 2 cot2 θ 1 ż 2 T 2 q 2 ẋt 2 +ẏ 2 T 2 1 q Inserting this into 3-37 yiels g 2. The last estimation functions t 4 for {φ 2,θ 1,θ 2 } can be calculate through the function t 3 w by changing φ 1 φ 2, θ 1 θ 2 an. An for the covariance we also substituting T T.

5 IV. TRACK INITIALIZATION FROM RANGE AND AZIMUTH A. Description Traitional active raars give range measurements an sometimes only partial bearings. This is usually not a problem if the setup can be approximate by a 2-imensional interpretation. Even so, the increase uncertainty shoul be incorporate in the covariance, which is a goo reasoning to apply our approach. Again we will use k = 2 measurements to estimate the position an make statistical assumptions about the velocity. The measurement moel for range an aimuth is as follows, r = x x s 2 +y y s 2 + s 2 4 an φ efine as in 23. As in the section on bearings only Sect. III, the measurements are taken apart with time ifference T.Sincewehavekη >η xo, we will be able to choose four ifferent functions t i. B. Two Aimuths an One Range Using {r 1,φ 1,φ 2 } we get the same results for x, y as in the previous section, see 31,32. Solving for we get = r1 2 x + /22 y 2 = r1 2 sin φ 2 sinφ 2 φ 1 T ẋ sin φ 2 2 ẏ cos φ 2 46 sinφ 2 φ 1 an accoringly t 1 w varies from 34 only in the last component. The lineariations necessary to compute the covariance can be foun in App II. To use the Unscente Transform UT we nee g. Inthis case the function is reaily available in 31, 32 an 46, which leas to immeiate applicability. The function t 2 can be generate from t 1, by replacing r 1 r 2, φ 1 φ 2 an as the setup is symmetric again. To calculate the covariance also T T has to be exchange. C. One Aimuth an Two Ranges To calculate t 2 w, weuse{r 1,r 2,φ 1 }.Firstweuse r 1, r 2 which is geometrically speaking the intersection of two spheres. Solving for x an taking the expectation over ẋ, ẏ, ż, we get, x = r2 1 r2 2 +2σ2 v + σ2 h T The solution for x is unambiguous since the intersection is a circle normal to an centere on the line connecting the centers of the spheres, which coincies with the x-axis. To solve for y, we use the efinition of φ, y =x + /2 tan φ 1 = r2 1 r2 2 +2σ2 v + σ2 h T tan φ 1 48 an similarly the efinition of r to solve for, = r1 2 x + /22 y 2 r = r r2 2 +2σ2 v + σ2 h T tan 2 φ 1, 2 49 where we isregar the ambiguity towars ±, since we can assume positive. Finally the complete function is, ρ 1 cos φ 1 /2 t 3 w = r ρ 1 sin φ ρ 2 1 with ρ 1 = r2 1 r2 2 +2σv 2 + σh 2T cos φ 1 We can see that if the component turns complex, there is no solution. This usually happens if the raii on t rener an intersection of the two spheres or the aimuth is off too far, ue to measurement errors or the unknown spees. The lineariations to calculate the covariance can be foun in App. II. The function t 4 can be generate from t 3 again, by replacing r 1 r 2, φ 1 φ 2 an ue to the symmetric setup. To calculate the covariance also T T has to be exchange. V. NUMERICAL RESULTS A. Bearings-Only Measurements As escribe in Section III, the sensors are locate at ±/2, where we choose = 1km for our numerical example. We plot results for an symmetric x/y half-plane of 4km by 2km an pick a constant height for each simulation. The two sets of measurements neee in this scenario are taken with an arbitrary time ifference T which is usually in the range of a few secons an the target is assume to move with constant spee within this time interval. The spees are ranom with ẋ, ẏ, ż assume inepenent an ẋ, ẏ RMSPOS in m Fig. 2. Root-mean-square position error RMSPOS for an meium target height =4km an synchronous measurements T =s. 2

6 RMSPOS in m Fig. 3. Root-mean-square position error RMSPOS for target height = 4km an asynchronous measurements T =2. 2 NEES Fig.. Normalie estimation error square NEES for target height = 4km an asynchronous measurements T =2s. The 9% acceptance region is [, 787; 6, 216], the covariance is about 1% too pessimistic. 2 RMSPOS min in m Fig. 4. Cramer-Rao lower boun CRLB for target height of =4km an asynchronous measurements T =2s. with the same σ v = 1m/s an ż with σ h =1m/s. Other parameters are the measurement noises, σ φ, σ θ, which are both ra.1 an the number of Monte-Carlo runs N =1 3. The covariance are generate using the Extene Kalman filter lineariation. For T = the measurements are not epenent on the unknown spee. The root-mean-square position estimation error RMSPOS erive by Monte-Carlo simulation for a height of = 4km is plotte in Fig. 2. The RMSPOS is minimal in close proximity to the sensors; it increases with istance to the sensors, consierably less so on the y- axis, when the base spanne by sensors is orthogonal to the bearings. When plotting the RMSPOS for non-ero T see Fig. 3, the average error is noticeably higher, ue to the influence of the unknown velocity. As a comparison we plot the CRLB for these values in Fig. 4. It can be observe that the estimation error meets the lower boun very well, so the increase estimation error variance is inherent to the problem setup. The consistency is evaluate using the normalie esti- 2 mation error square NEES, which is calculate using the full estimation error vector. Even though x o is not actually estimate, we inclue it in the NEES calculation to check the consistency of the cross-correlation. Accoringly the NEES shoul have the istribution of a chi-square istribution with η x N =6 1 3 egrees of freeom. The NEES is plotte in Fig. ; it is slightly below the 9% acceptance region, which makes our estimate covariance pessimistic. On top of the sensor positions the estimates are inconsistent, which is a problem of calculating the covariance with ero groun range. For larger heights, e.g., =8km the error is more even. It is higher aroun the sensors, since the minimum istance is limite by the increase height. Also it rises less on the x-axis, since it has now higher elevation - which improves the performance initialiing from one aimuth an two elevation. Decreasing the height to = 1km, the errors increase; especially on the x-axis, outsie the sensors. This is a problem inherent to the estimation geometry; on the x-axis in general t 1,t 2 on t work, since intersecting two glancing aimuth angles makes the estimation accuracy ten towars ero. For low heights, t 3,t 4 work baly on the x-axis outsie the sensors, since now the measure elevation angles glance. B. Range an Aimuth Measuements The setup for position initialiation from two measurements is basically the same as in Section V-A. The sensors are on the x-axis with istance =1km, we simulate over a halfplane, the assume istribution of the velocity is the same, noise variance is σ φ.1 an σ r =6m. We use the Unscente Transform UT to calculate the covariance. We will look at a larger height of =8km an time offset of T =2s. The RMSPOS is plotte in Fig. 6; for the chosen range stanar eviation of σ r = 6m an same aimuth precision, the estimation error is generally higher compare to the bearings-only scenario. Especially for positions far from the sensors the errors increase much stronger. As a comparison we plot the CRLB in Fig. 7. We observe that the stronger

7 2 RMSPOS in m Fig. 6. Root-mean-square position error RMSPOS for larger target height =8km an asynchronoeous T =2s measurements of range an aimuth. 2 NEES Fig. 8. Normalie estimation error square NEES for target height = 8km an asynchroneous T =2s of range an aimuth. 2 RMSPOS min in m Fig. 7. Cramer-Rao lower boun for target height =8km an asynchroneous T =2s measurements of range an aimuth. increase is inherent to the setup as we can not go beneath the boun, but in corners where the errors increase strongly, our estimation errors o not achieve the CRLB. Fig. 8 shows the consistency. It is less pessimistic, but there are regions where the very large errors lea to inconsistencies, coinciing with where the estimation error oesn t achieve the CRLB. For lower heights, e.g., =4km, the errors increase, in particular in the regions of high errors. Again this is inherent to the probelm geometry can not be improve upon, which we valiify via CRLB analysis. Therefore in this scenario our approach has only limite applicability. It works well for short ranges an high elevations, while for large range an low elevations a 2D approximations is justifie. C. Implementing the Covariance Matrix Lineariation an Unscente Transform are both methos to hanle non-linerarities. Sometimes none of these methos is able to generate a suite covariance matrix. Generally for big measurement errors an/or strong abberation of the moele parameter from the expecte parameter, we notice 2 enlarging regions which become inconsistent. The Unscente Transform seems to work slightly better with this problems than lineariation, which can be observe in the case of bearings only measurements with larger measurement errors σ φ = σ θ = 1 o an in the case of range an aimuth measurements. VI. CONCLUSION We provie a general approach for initialiation, applicable to a large range of estimation problems using incomplete measurements. The approach allows moeling some parameters statistically to reuce the imension of the estimation problem. This way we can initialie also in scenarios where observability woul otherwise not be given. A covariance matrix has been erive analytically to escribe the estimation errors. The covariance accounts explicitly for uncertainties relate to reucing the estimation problem which is our main contribution. We inclue a etaile iscussion an numerical analysis of examples using Cramer-Rao lower bouns an Monte Carlo Simulation. The examples inclue 3D position initialiation scenarios of bearings-only an slant-range an aimuth. APPENDIX I LINEARIZATIONS FOR BEARINGS-ONLY SCENARIO A. Two Aimuth, One Elevation Lineariing 34 we get t 1 w = sin φ2 cos φ2 sin 2 φ 2 φ 1 sin2 φ 2 sin φ1 cos φ1 sin 2 φ 2 φ 1 sin 2 φ 2 φ 1 sin2 φ 1 sin 2 φ 2 φ 1 φ 1 ρ 1 / cos 2 ρ θ 1 1 φ 2 where ρ 1 = x y 2 an = 1 x x + /2 + y y,i=1, 2 φ i ρ 1 φ i φ i 2

8 Instea of taking the erivative of h towars x to apply the chain rule, we can irectly take the erivative of Since G = t h x o, we only nee to take the erivative towars x o which the equations are alreay linear in. It is easy to see that t 1 h T = x o sinφ 2 φ 1 cos φ 1 sin φ 1 sin φ 2 cos φ 2 3 which gives everything necessary to calculate the covariance accoring to 16. B. One Aimuth, Two Elevation t We calculate the lineariation 3 t3 h w an x. 3-37, which all epen on ρ 1 from 4, have the lineariation in w given in 4, with the following partial erivatives q 2 sin φ 1 ρ 1 = φ 1 q 2 cos φ 1 +1 q 2 ρ 1 = ρ2 1 2 cos φ 1 ρ θ i q 2 cos φ 1 +1 q 2 q q i =1, 2 6 ρ 1 θ i q = tan θ 2 q 1 θ 1 sin 2, = θ 1 θ 2 cos 2. 7 θ 2 To linearie t 3 h in x o we will linearie 39 aroun T =. We fin T = ρ 1 cot θ 1 ż q 2 ρ 1 cos φ 1 ẋ q 2 ρ 1 sin φ 1 ẏ ρ 1 1 q 2 +q 2 cos φ 1 8 which is linear in ẋ, ẏ an ż. ForsmallT we can approximate ρ 1 by ρ 1 = ρ 1 T = + T ρ1 T. Defining N = ρ 1 1 q 2 +q 2 cos φ 1 9 an using 3-37, the result is t 3 h = T cos φ 1 sin φ 1 x o q2 ρ cos φ N q2 ρ sin φ 1 N where ρ = ρ 1 T = is the solution to 4. ρ cot θ 1 N 6 APPENDIX II LINEARIZATIONS FOR RANGE AND AZIMUTH SCENARIO A. Two Aimuth, One Range To calculate the covariance matrix, we alreay have most erivatives from the previous section, but still nee to linearie 46. The result for t 1 w = 2 sin φ2 cos φ2 t 1 w is sin 2 φ 2 φ 1 sin2 φ 2 sin φ1 cos φ1 sin 2 φ 2 φ 1 sin 2 φ 2 φ 1 sin2 φ 1 sin 2 φ 2cosφ 2 φ 1 sin 3 φ 2 φ 1 r 1 2 sin 2 φ 2 φ 1 sin φ 1 sin φ 2 sin 3 φ 2 φ 1 where for we have to use our estimate. For t1 h x o 61 t 1 h T = x o sinφ 2 φ 1 cos φ 1 sin φ 1 sin φ 2 cos φ 2 62 sin φ2 sinφ 2 φ 1 B. One Aimutn, Two Range For the {r 1,r 2,φ 1 } case the erivatives to calculate the covariance are, t 3 w = ρ 1 r 1 r2 r 1 cos φ 1 tan φ 1 r2 tan φ 1 tan φ1 1 ρ1 ρ 2 1 r 1 cos 2 φ 1 t 3 h x o = T 2 2 cos φ 1 1 tan φ 1 x+/2 r 2 ρ 1 2 cos φ tan 2 φ 1 2x 2y 2 64 where for ρ an x, y, the estimate values have to be use. REFERENCES [1] K. Becker, Target motion analysis aus Winkelmessungen: Parametrische Stuie in rei Dimensionen, FGAN, Wachtberg-Werthoven, FKIE Bericht 12, 2. [2] Y. Bar-Shalom, X. R. Li, an T. Kirubarajan, Estimation with Application to Tracking an Navigation. Wiley-Interscience, 21. [3] G. van Keuk, Ein Basisalgorithmus für ie räumliche Triangulation, FGAN, Wachtberg-Werthoven, FFM Bericht 418, [4] T. Kirubarajan, Y. Bar-Shalom, an D. Lerro, Bearings-only tracking of maneuvering targets using a batch-recursive estimator, IEEE Trans. on Aerospace an Electronic Systems, vol. 37, no. 3, pp , Jul. 21. [] S. J. Julier an J. K. Uhlmann, Unscente filtering an nonlinear estimation, Proceeings of the IEEE, vol. 92, no. 3, pp , Mar. 24. [6] H. Van Trees, Detection, Estimation, an Moulation Theory, 1st e. New York: John Wiley & Sons, Inc., t 3 w = φ 1 cos φ 1 ρ 1 sin φ 1 θ 1 cos φ 1 φ 1 sin φ 1 + ρ 1 cos φ 1 θ 1 sin φ 1 φ 1 θ 1 + ρ1 cos 2 θ 1 θ 2 cos φ 1 θ 2 sin φ 1 4 θ 2