Sie sind auf Seite 1von 11

1.

Introduction

Kalman Filter Behavior in Bearings-Only Tracking Applications


VINCENT J. AIDALA, Member, IEEE Naval Underwater Systems Center

Passive localization and tracking techniques are of interest in a variety of sonar applications [14]. In the ocean environment, two-dimensional bearings-only target motion analysis (TMA) is perhaps most familiar [4, 5]. Here, a moving observer (own ship) monitors noisy sonar bearings to an acoustic source (target), and subsequently processes these measurements to obtain estimates of source position and velocity. The geometric situation is depicted in Fig. 1, where both own ship and target are presumed to lie in the same horizontal plane (underline symbols correspond to boldface symbols in the text).

Abstract
The extended Kalman filter applied to bearings-only target tracking is theoretically analyzed. Closed-form expressions for the state vector and its associated covariance matrix are introduced, and subsequently used to demonstrate how bearing and range estimation errors can interact to cause filter instability (i.e., premature covariance collapse and divergence). Further investigation reveals that conventional initialization techniques often precipitate such anomalous behavior. These results have important practical implications and are not presently being exploited to full advantage. In particular, they suggest that substantial improvements in filter stability can be realized by employing alternative initialization and relinearization procedures. Some candidate methods are proposed and discussed.

(NORTH)

Vt
|
X-

I
vS

^ TARGET
R

ON SHIP
X (EAST)

Fig. 1.

Unfortunately, this particular estimation problem is not amenable to simple solution. Nonlinearities in the measurement equation preclude the use of conventional linear analysis, although pseudolinear formulations are possible [6-8] . Furthermore, since bearing measurements are extracted from a single own ship sensor, the problem is illposed in the general case of arbitrary target motion; indeed, unique solutions exist only for nonmaneuvering targets. Even when target motion is suitably constrained, the dynamic process remains unobservable prior to an own ship maneuver
Despite the aforementioned difficulties, numerous techniques have been developed for bearings-only target tracking [9] . One method of solution which has received considerable attention in recent years is the extended Kalman filter [10]. Although this procedure generally yields satisfactory results, there is documented evidence of erratic filter behavior [5, 7, 1 1 ]. In particular, premature collapse of the error covariance matrix has been observed, even under favorable operating conditions. This anomaly is especially detrimental to system performance since it often leads to solution divergence. As a result, reliable TMA estimation via the extended Kalman filter cannot always be assured. Unfortunately, previous attempts to prevent abnormal filter behavior have proved to be ineffectual when subjected to rigorous testing [11 ] . These approaches seem to fail mainly because they focus only on the symptoms of
VOL. AES-15, NO. 1

[51

Manuscript received May 25, 1977. Author's address: Code 35201, Naval Underwater Systems Center, Building 1171, First Floor, Newport, RI 02840. 0018-9251/79/0100-0029 $00.75 O 1979 IEEE

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

JANUARY 1979

29

the problem; the underlying causes, which are perhaps not well understood, have been neglected. Here, we demonstrate how the interaction of bearing and range estimation errors can induce premature covariance collapse and subsequent filter divergence. Further investigation reveals that such erratic behavior is frequently precipitated by the use of conventional initialization techniques. These results have important practical implications and are not presently being exploited to full advantage. In particular, they suggest that substantial improvements in filter stability can be realized by employing alternative initialization and relinearization procedures. Some candidate methods are proposed and discussed.

G(k) = P(k k - 1) H'(k) [H(k) P(k I k - 1) H(k)


+

u2(k)fl'
k-

(2d)

X(klk) = X(klk - l)+ G(k){f(k)-h[X(kik - 1)]} (2e)

P(kik) = [I - G(k) H(k)]P(k

1),

k=

1, 2, 3, ...

(20)

where the "prime" symbol (') denotes matrix transposition. Here, X(iij) and P(iIj) for i, j = 1, 2, 3, ... denote estimates of the true state vector X(i) and its associated error covariance matrix P(i), respectively, based on j data measurements. Although equation set (2) is useful for numerical work, it is not readily amenable to analysis. In contrast, nonrecursive representations of the filter are often more tractable, despite 11. Description of Filter Equations their computational inefflciency. One such representation is obtained by reformulating the original dynamic estimaThe bearing-only TMA problem can be mathematically described by a linear state model and a nonlinear scalar mea- tion problem for X(k) as a static estimation problem for X(0). The resulting algorithm is given below [1 2]: surement equation of the form'
k
= A(k,

X (k)

) X(k - 1) -W (k)
k= 1,2,3
...

(1 a)

X(k l k) = A(k, O) X(OI k)- z A(k, j) Wfj)

j=1

:(k) = h [X(k)] + r?(k)

(1 b)

k=1,2,3,...

(3a)

where X(k) is a four-dimensional state vector which describes the target motion parameters at time kT, A(k, k - 1) is a 4 X 4 deterministic transition matrix, W(k) is a four-dimensional vector of deterministic inputs, ,6(k) is the noisy bearing measurement taken at time kT, and r?(k) is the additive measurement noise. It should be noted that (la) is developed under the assumptions of zero plant noise and constant target velocity. Although the plant noise restriction is unnecessary, it is imposed here to simplify the analysis. Further simplification also results by assuming that 71(k) is a zero mean white noise process with variance u2 (k). Finally, for completeness, initial estimates of the state vector and its associated error covariance matrix are presumed to be specified. Because of nonlinearities in (lb) direct application of the conventional Kalman filter is not possible. Approximate algorithms, such as the extended Kalman filter [10], must be employed. For the problem under consideration this particular estimation scheme takes the form

P(klk)=A(k, 0) P(OIk)A'(k, 0)
where

(3b)

X(0 1 k) = P(01 k) P-1 (010) X(0 0)


k

+ P(O I

k)

j=l
k
y

M'(f)-2(j) {,Bq) -h[X(] ij - 1)]}


j
Y

+ P(O I k)

j=l i=1

M'Cq)a~2 (i) M(t) A (0, i) W(i),


k= 1,2,3,...

(4a) (4b)

P(O I k) = [p- 1 (0 1 0) + y Mt(q)a- 2 (i) M(Z)l]1=l and

A;(Q) = H(j) A(j, 0),

j= 1,2,3,...,k.

(5)

It should be emphasized that equation sets (3) and (4) are

X(0 0) = initial estimate of the state vector


P(0 0) = initial estimate of the state vector error covariance
matrix

mathematically equivalent to equation set (2); accordingly, they provide an exact description of the extended Kalman filter as applied to bearings-only target tracking.
111. Behavior Characteristics of the Filter

X(k

lk -

1) =A(k, k

1) X(k

1 k-

1)--W(k)

P(k k - 1) =A(k, k - 1) P(k - 1 I k - ) A'(k, k - 1)

In operational versions of the extended Kalman filter, (2b) X(k k) and P(k k) are of particular interest since they

(2a)

H(k) = ah/aX IX=X(kIk-1)

(2c) for the purpose of analysis, it is more convenient to work

statistically characterize the current state vector. However,


directly with X(0 k) and P(O k) which do not vary explicitly with time. Although this approach is somewhat circuitous, it actually poses no difficulty since all four quantities are related by equation set (3). In fact, the behavior
VOL. AES-15, NO. 1 JANUARY 1979

'Details of the modeling process are presented in Appendix A, along with a precise description of the various parameters.
30

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

of X(kl k) and P(k k) can be determined forthwith from knowledge of the behavior of X(0 Ik) and P(O k). To begin the discussion, consider the closed-form expression for P(O k) given by (4b). If the matrix M(Q) is explicitly evaluated via (5), it can be shown that

P' (01 0) as a 4 X 4 null matrix. In this situation (1 1) reduces to

P(O I k) - P(OI k) = C(k)

(13)

1=l1

z M(j)a-2 (f) MC) = {Q-1 (k) B(k)}' {Q- 1 (k)B(k)} = C(k)

(6)

where C#(k) is the matrix pseudoinverse of C(k) [13]. However, observe from (12) that the matrix product P(O k)P- 1 (01 0) does not vanish, but rather is transformed into

where C(k) is a 4 X 4 symmetric positive semidefinite matrix (commonly referred to as the observability matrix), and

P(01 k)P'-(01 O) = I - C#(k) C(k).

(14)

2(k) diag [21 f22 2,


=

1,

...

2kki
i= 1,2,3,...,k

(7a)
(7b)

Substituting (13) and (14) into (4a) yields the expression

Rii =R(iIi- 1)a(i),


cos3(liO)
B(k) = L
cos

X(0 k) X(0 k) = [I - C-()Ck]XOlO


-->

,(2 1 1)
*

-sin,B(ll0) -sin 3(21 1)


0

C#(k)

1=1
k

M()G-2 () {3()
j
=1

h [X lj- 1)] }

C j(k)

j= 1i= 1

(i)

-2 0 MO A(O,

i) W(k).

(15)

Lcos P(kl k- 1)
1 cos3(110) 2 cos ,(21 1)

-sin 0(k k- 1)
-1 sin,B(110) -2 sin (21 1) t

(8)

k cos (kl k- 1)

-k sin j(k l k-

1)j
(9a)

R(klk - 1)=tR2 (kik- 1)+

Ry(ki
3'

k-I)]

2(k R(k k - 1) = -vIR X k -1) + P`-(k k - 1)


and where it is implicitly assumed2 that

(9b)

Equations (13) and (15) describe the estimated state vector X(0 k) and the covariance matrix associated with its observable components [i.e., P(0 k)] for the case of totally unreliable a priori data. It is noteworthy that these results cannot be extracted directly from the recursive filtering algorithm despite equivalence between equation sets (2), (3), and (4). An apparent discrepancy exists because utilization of the recursive equations implicitly requires that P(O 0) be finite, thus precluding the selection 0 of P- l (0 O) as a null matrix. To circumvent this difficulty assume that P(O l 0) is chosen in accordance with the formula

P(OI O0) = a2I

(16)

R(klk- 1) a(k) # 0.
If (4b) and (6) are combined, it follows that

where ao is an arbitrary nonzero scalar, and I is the 4 X 4 (10) identity matrix. If (16) is substituted into (11), it can be shown that [14]

P(OI k) = [P-'(010) + C(k)]'1


or,

(1 1)

P(O I k) = P((01 k) +PO(01 k)


where
P

(17)

equivalently,
=

P(O k) {I [P- (0 0) C(k)l C(k)} P(O 0).


1

(12)

(0

k)

a2 [I - C#(k) C(k)]

(18a)

and

In most practical tracking applications accurate initilalization data are usually unavailable; thus an examination of filter behavior under such circumstances is of particular interest. The limiting case of totally unreliable initialization data can be mathematically modeled by choosing

P0(O i k) = [I+
=
-

a2 C#(k)]1C#(k)

C#(k) a2 C#(k)[I + U2 c# (k)] - 1 C# (k).

(18b)

As might be expected, Pu(0 k) and PO(0 k) are the covariance matrices associated with the unobservable and observable components of X(0 k), respectively. Although the matrix relationship embodied in (17) is 2This assumption will always be satisfied under realistic operating perhaps unfamiliar, it can be easily verified by multiplying both sides of the expression with the matrix [<To2I + C(k)], conditions.
AIDALA: KALMAN FILTER BEHAVIOR IN BEARINGS-ONLY TRACKING APPLICATIONS

31

and subsequently utilizing the pseudoinverse identities [13]

C#(k) C(k) -Qk)C(k), for C'(k) = C(k)

(19a) (19b)
(19c)

Accordingly,

which follows from (6) and the fact that Q(k) is nonsingular.

C#(k)C(k)C*(k)= C(k)

C#(k) C(k) * I

rank [B(k)J < 3 rank [B(k)]


= 4.

(25a)

C(k)C"(k)C(k)

CQk).

C#(k) C(k) = I

(25b)

Furthermore, this representation for P(0 k) will remain valid even when the dynamic process is unobservable (i.e., prior to the first own ship maneuver). Combining the foregoing results with (4a), and noting that3
PU(O k) J;
k k

Substituting equation sets (18) and (25) into (17) then yields the result

P(O k)

Po (0( ik) + P
= P0

(0 I k)

rank [B(k)] .3
rank [B(k)]
=

(0 I k)

4.

(26)

j=1

I MIQ)Ua 2 0i){pqO-h[XCl j-1) }- 0


i=1

(209.a)
(

U(01 k) 2;

j=l

2 M(i A' MOA(OIi) W(i)0

Previous theoretical studies of the bearings-only TMA problem [51 have shown that, in the absence of estimation errors, B(k) attains full rank only after own ship executes a b) maneuver; i.e.,

leads to the following expression for X(01 k):

rank [B(k)] S 3
(2: 1)

before the first own ship maneuver after the first


own

X(01 k) = X(01 k) + O(aj2).

rank[B(k)]

ship maneuver.

Here, the symbol o(ai2) is used to designate that all terms not explicitly included in (21) are of order ao2 or higher. A similar relationship for P0(0 k) may be deduced from (13) and (18b); i.e.,

P0(O I k) = P(0I k) +

(a2 ).

(22)
-

The remaining term in (17) is also seen to exhibit consistent behavior as a function of a0 ; viz., P,(0 1 k) O(aJ ) which reflects the fact that errors in the unobservable components of X(0 k) are directly proportional to X(0 1 0). Consequently, when P(0 1 0) satisfies (16) for large values of ao estimates obtained via the recursive filter will approximate the theoretical solution associated with totally unreliable initialization data; namely,

X(0 1 k)

X(0 1 k)
02

(23a)

P(01 k)

[OI - (k) C(k)]1 + P-(O k)-

(23b)

As a final note, observe that the right-hand side of (23b) represents the complete covariance matrix of X(0 k) [i.e., the covariance matrix associated with both observable and unobservable components of X(0 k)]. Equation (17) is especially useful for determining the behavior characteristics of P(0 k), -and for identifying one of the principal causes of premature covariance collapse. To analyze this expression first observe that
rank [C(k)] =rank [B(k)]
4

(24)

pseudoinverse identities described in [ 13 ].

'These two relationships can be readily established by expressing the pertinent series in matrix notation and then utilizing standard
32

Note that the condition rank [B(k)] < 3 characterizes unobservable estimation process; in this situation, bearing measurements alone do not provide sufficient information to uniquely determine X(0 1 k). Consequently, prior to the first own ship maneuver, PJ(0 1 k) should behave as a nonnull matrix. Moreover, if large values of ao are used for filter initialization, it follows from (26) that Pu(0 k) should also contribute the dominant eigenvalues to P(0 k) whenever rank [B(k)] < 3. This can be seen more clearly by examining equation set (18), keeping in mind that C(k) is independent of a0. In particular, observe that the eigenvalues of PJ(0 i k) either vanish or are identically equal to a2 Furthermore, the eigenvalues of P(0 1 k) have a one-toone equivalence with the nonzero eigenvalues of Pu(0 k) i and P0(O k). If X(k) denotes the smallest nonzero eigenvalue of C(k), then the largest eigenvalue of PO (01 k) will be equal to [a&2 + X(k)f-1. Thus, for sufficiently large values of ao {i.e., ao > X-1 (k)}, the relation [maximum eigenvalue of PU(0 k)] > [maximum eigenvalue of PO (01 k)] holds provided that rank [B(k)] < 3. From the preceding discussion, it is evident that P(0 1 k) will collapse prematurely {i.e., its dominant eigenvalue will decrease in magnitude from a2 to [ao2 + X(k)fl } if B(k) attains full rank prior to the first own ship maneuver. When (16) is satisfied such anomalous behavior on the part of B(k) is possible only if the bearing estimates 3(k k 1) are in error. Indeed, the condition rank [B(k)] = 4 requires that all four column vectors in (8) be linearly independent; however, under the prescribed circumstances, this requirement will be impossible to satisfy if the estimated bearing coincides with its actual value at all instants of time. Stated another way, collinearity of the vectors R(i l 1-) and R(/) (see Fig. 2) will eliminate the possibility of premature covariance collapse.
an
. -

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. AES-15, NO. 1

JANUARY 1979

when initialized with (27) and (28) as opposed to (16). In particular, since S(k) does not approximate C(k) unless ar and a, are both large, the estimated state vector and covariance matrix no longer approximate the theoretical solution associated with totally unreliable initialization data. Moreover, P(0 1 k) will collapse prematurely, even in the absence of bearing estimation errors. To see this, observe from equation set (32) that the dominant eigenvalue of P(0 l k) equals a, whenever rank [S(k)] < 3. However, if the predicted bearing rate is nonzero {i.e., 3(k + 1 1 k) # 3(kI k - 1) for k = 1, 2, 3, ...} this rank condition will be satisfied if and only if k < 2. For k > 2, the matrix S(k) attains full rank and is strictly positive definite. In that case, the domi(27) nant eigenvalue of P(0 1 k) decreases in magnitude from a, P(01 0) = diag [a2,a', a2,u2 ] to {ar2 + X(k)}- l, where X(k) > 0 represents the smallest with eigenvalue of S(k). It should be noted that the covariance behavior described >av>. here is induced entirely by assigning unequal variances to the (28) ar components of X(0 1 0). Another undesirable property of To examine covariance behavior under these initial condithis initialization procedure is that it causes the eigenvectors tions, (27) is substituted into (11) and manipulated to yield of P(0 1 k) to differ from those of C(k). Furthermore, when ar and a, satisfy (28), P(0 1 0) becomes ill conditioned [19]; 401 k) = [a&21+ S(k)]1 (29) thus, subsequent computations involving this matrix will r be susceptible to numerical errors. In essence, even though where (27) and (28) may be "theoretically" acceptable, they are inherently unsuitable for practical tracking applications. 1 0 O Indeed, the initialization procedure defined by these equaS(k) = L + C(k). (30) tions not only fails to adequately model unreliable initialization data, but actually precipitates covariance collapse. O (a-2 _ a-2)I The effect of anomalous covarinace behavior on X(0 k) Hlere, the first term on the right-hand side of (30) has been will now be determined. To this end, first assume that the block partitioned into appropriate 2 X 2 submatrices. Since filter is initialized via (16). Under these conditions, X(0 i k) S(k) is symmetric and positive semidefinite it follows from will satisfy (21); thus, for large values of ao, the approximate previous results [see ( 11){(18)] that expression (31) X(0 k) k(0 k) POlk)=QU(0Ik)+ Qo(OIk) X (33) where follows directly by neglecting terms of order ao2 or higher. (32a) Properties of the estimated TMA solution may be QU (Oik)= a2 [I S#(k) S(k)] r
AIDALA: KALMAN FILTER BEHAVIOR IN BEARINGS-ONLY TRACKING APPLICATIONS

Another noteworthy fact revealed by the analysis is that range estimation errors will not alter the rank of B(k) and, therefore, cannot initiate covarinace collapse. Consequently, ad hoc schemes which rely solely on range constraints to prevent abnormal filter behavior are actually focusing on the wrong parameter (simulation results presented in [1 1 ] support this finding). The utilization of such schemes has apparently evolved from a misconception that abrupt variations in estimated target range precipitate covariance collapse; in reality just the opposite is true. These remarks are not meant to diminish the significance of range estimation errors, but rather to emphasize that their impact on the filter computations becomes most consequential only after the onset of covariance collapse. In any case, the importance of maintaining bearing accuracy during all phases of the estimation process should be clearly recognized. So far the examination of P(0 k) has been restricted to the special case where P(0 1 0) satisfies (16). Unfortunately, this particular initialization scheme is not widely used in bearings-only tracking algorithms, even though its attributes are well documented [14-17]. Moreover, results from earlier TMA studies [7], and recent extensions thereof [18], suggest that the application of (16) will actually enhance filter stability. One possible explanation is that the covariance initialization parameters have minimal effect on subsequent filter computations [observe that ao appears in equation set (18) only as a multiplicative constant, and does not otherwise distort the observability matrix C(k)]. Despite the practical utility and computational advantages of employing (16), most operational TMA algorithms specify P(0 1 0) as a diagonal matrix in the form

TARGET POSITION (ACTUAL)

TARGET POSITION (PREDICTED)

OWN-SHIP POSITION

Fig. 2.

and Q (0 i k) = [I +a-2S*(k)] -1S#(k)


=

S#(k) - a-7S#(k) [I + a- 2S#(k)] -1 S#(k)

(32b)

(31), the recursive filtering algorithm behaves quite differently

While there are obvious similiarities between (17) and

33

deduced by analyzing the individual terms comprising X(0 1 k) [see (15)] . Notice that the first term vanishes when rank [B(k)] = 4, and will not affect X(0 k) after own ship executes a maneuver. Conversely, the third term does not influence X(0 k) before the first own ship maneuver because WQi) is a null vector during that period (see Appendix A). While both these terms are significant, their respective contributions to X(0 k) are mutually exclusive. In contrast, the second term of (15) is always present, but rarely dominant. Indeed, this particular term is simply a weighted sum of bearing measurement residuals that should approximate zero mean white noise [10]. Previously, it was shown that premature covarinace collapse occurs whenever B(k) attains full rank prior to the first own ship maneuver. In such a situation, the first term of (15) will vanish; however, since own ship has not yet maneuvered, the third term remains unaltered (i.e., identically equal to zero). As a result, X(0 k) undergoes an abrupt change from its original value to a new value given by
k

As might be expected, the last two terms of (35) behave nuch like their respective counterparts in (15). However, the remaining terms, which explicitly depend upon X(0 0), exhibit quite different properties. Specifically, the first term in (35) vanishes for k > 2, even in the absence of bearing estimation errors or own ship maneuvers. In essence, the initial range estimate R(O 0) is discarded after two measurements are processed, and subsequently replaced with a new estimate which depends only on V(0 0) and the measured bearings. Unless these estimates are mutually compatible (i.e., the predicted bearing rate accurately approximates the measured bearing rate), the range components of X(01 k) will abruptly change when k > 2. Unfortunately, abnormal variations in estimated target range can distort the data measurement weighting pattern of the extended Kalman filter and precipitate further covariance collapse. An examination of (6) and (7) reveals that R( I j - 1) enters into the covarinace computations through 2-1 (k), but only as a multiplicative factor on a(j). The pertinent elements of f21 (k) may be expressed in the form

X(0 I k) C#(k) 22 -Mh(f)cr2 ){@(j)


-

(34) The fact that this new value is typically small explains why a substantial reduction in estimated range is often observed following the onset of covariance collapse. When (27) and (28) are used for covariance initialization lieu of (16), the estimated state vector experiences an in additional degree of instability. To illustrate this, first observe that equation set (20) remains valid4 when P"(0 l k) is replaced by Qu(0 k). Consequently, (4a) and (31) may be combined to yield the approximate expression

/=1

[X(iIj

)]

[R(jlj - 1) u(j)] -1

= [RQ)/RQ I j - 1)]

[R(Q)a(j)] -1,
(37)

j 1, 2, 3, ...k

X(O k)

[I - S#(k) S(k)]

R(0 0)

L i ]
+

av S`(k)

---------

TVO O)

+S# (k) .2 M'()-2(j){3(j) - h [X(jl/j- 1)]}


j=1
k

22 S#(k) j=l

E
i=l

M'Q)u-2(f)M(t)A(Oi)W(i)
+

where
S(k) 5- --------0 la-2 I
-

where RQ) is the actual range at the jth instant of time, and [R(j)a(/)]-1 is the correct weighting factor associated with the jth bearing measurement. If the term [R(Q)/RQ j - 1)] varies slowly with time, Q l'(k) will be an approximate scaled replica of its true value; in this case, the correct relative weighting between successive bearing measurements is preserved. The presence of a scale factor may alter solution convergence time, but it should not degrade steady state performance. Alternatively, if [R(j)/RUl j - 1)] becomes abnormally large, the corresponding measured bearing will appear to be almost error free. The filter subsequently attempts to adjust the estimated solution so as to "match" this bearing, even at the expense of mismatching the previous measurements (which appear to have much larger error by comparison). After a certain number of these "error-free" bearings have been processed, the covariance matrix becomes completely collapsed, thus preventing any further correction of the estimated solution. Because the bearings-only TMA algorithm presently has no mechanism to prohibit abnormal variations in the term [R(U)/R(i j - 1)1, covariance collapse is usually fatal.
IV. Pseudolinear Formulation of Bearings-Only TMA Problem

The preceding analysis has been devoted mainly to identifying the causes of abnormal behavior in the bearings-only TMA algorithm. As a natural extension, we now focus on Here, X(0 0) has been block partitioned into range and some practical techniques to improve algorithm stability. The normalized velocity subvectors (see Appendix A); in addifirst and most obvious technique is to employ (16) for tion, all terms of order ar,2 or higher have been neglected. 4Although these identities involving Qu(0 k) are not immediately covariance initialization. Indeed, this modification not only eliminates a potential source of fillter instability, but obvious, they can be established by evaluating S (k) in block partitioned form via (30) and then substituting the results in (32a). also provides a more suitable representation of unreliable
34
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. AES-15, NO. 1

Qk).

(36)

JANUARY 1979

initialization data. Next, it is necessary to decouple the G(k) =P(klk-1)H'(k)[H(k)P(klk-1)H'(k) + a2(k)]-' covariance computations from the estimated state vector (44c) so as to prevent feedback and subsequent amplification of solution errors. One of the simplest decoupling techniques P(klk)= [I-G(k)H(k)] P(klk- 1) (44d) involves replacing measured bearings with pseudolinear measurement residuals [6-8, 18]. An attractive feature of this and approach is that it permits a solution to the bearings-only TMA problem via linear estimation theory. To see this, a(k) = {R(k k - I)/ao} a(k). (45) observe that equation set (Al 1) in Appendix A may be algebraically manipulated to yield Observe that neither ao nor R(k k - 1) explicitly appear in equation set (44); consequently, these parameters will 0 = H(k) X(k) + R(k) sin X(k) (38) not affect the normalized gain and covariance computations unless a (k) is evaluated via (45). However, strict application where of this equation is unnecessary because a (k) serves only to weight the kth data measurement relative to the initial state H(k) = [cos 3(k), -- sin j(k), 0, 0] (39) vector estimate. Under realistic operating conditions, such weighting is approximate at best, inasmuch as the true R(k)= R2 (k) +R2(k) (40) values of ao and R(k k - 1) are unknown. Thus, any other suboptimal scheme that yields reasonable weights may legitand 0(k) is the measured bearing at time kT. Here, the non- imately replace (45). In light of these remarks, (k) will be linearity has been embedded in the measurement noise. If computed via the formula
^

e(k) = R(k) sin 77(k) = effective measurement noise at


time kT

a (k) =

a(k).

(46)

(41)

and 71(k) is Gaussian distributed, it can be shown [7] that e(k) has the following statistics:
E[e(k)]
=
0

(42a)

E[e2 (k)]

[R2 (k)/2] [1

exp{-2u2 (k)}]

R R2(k)U2 (k),

o2 (k) << 1.

(42b)

Equations (la), (38), and (41) may now be combined to provide a pseudolinear model of the bearings-only TMA problem. The Kalman filter for such a model is analogous to the algorithm described by equation set (2); in fact, it may be derived from this equation set by replacing H(k) with H(k), replacing a(k) with R(k k 1)a(k), and replacing {@3(k)- h[X(klk- 1)]} with -H(k)X(kIk- 1). Furthermore, if the covariance matrix is initialized in accordance with (16), then
-

P(kl1 C-1 =O2P(kI k -1)


G(k) = G (k)

(43a)
(43b)

P(k I k)= OuP(k k)


-

(43c)

where P(k k 1), G (k), and P(k k) satisfy the normalized computational algorithm

P(0 0) = I = 4 X 4 identity matrix P(kik-l)=A(k,k-1)P(k-lIk-l)A'(k,k- 1)

(44a) (44b)

Despite its simplicity, (46) effectively decouples the covariance computations from estimated target range without seriously distorting the statistical weighting characteristics of the filter. This is especially true in surveillance and trailing applications, where the actual range variation is typically small. For these important TMA scenarios, the approximation [R(k k 1)/ao ] 1 can be satisfied by choosing ao = R(O 0); (46) then follows directly from (45). As an ancillary benefit, it is no longer necessary to specify ao, since this parameter will not be explicitly required for filter initialization. Another method to enhance filter stability is to initialize the state vector estimate in a consistent manner. In particular, since a unique TMA solution does not exist prior to the first own ship maneuver, it might be advisable to take precautions during this period to insure that the estimated solution is not misconstrued as being accurate. One procedure that effectively accomplishes this is to set X(0 l 0) equal to the trivial relative motion solution (i.e., estimated range and velocity are both equal to zero). It is fairly easy to demonstrate that X(kl k) will remain "locked on" to this trivial solution until own ship executes a maneuver [7]. Such a scheme inherently establishes a consistent and predictable behavior pattern for the filter; thus, erratic performance should be more readily discernible. Finally, all initialization is now prespecified in a well-defined manner. Incorporating the preceding results into equation set (2) leads to the pseudolinear filter for bearings-only TMA. The pertinent algorithm is summarized in Appendix B; a detailed analysis of its properties may be found in [18].
-

V. Simulation Results

To demonstrate the efficacy of the pseudolinear filter


35

AIDALA: KALMAN FILTER BEHAVIOR IN BEARINGS-ONLY TRACKING APPLICATIONS

^ 4400 INITIAL BEARING - 95 OWN-SHIP SPEED = 6. 5 TARGET COURSE - 320 TARGET SPEED : 25
INITIAL RANGE

YARDS DEGREES KNOTS DEGREES KNOTS


-4 38
-6
-8 _4
Wi

,CURSE ERROR= +70HERE

t
Fig. 5. Fig. 6.
OWN-SHIP TRACK

TARGET TRACK

Fig. 3.
z

Fig. 4.
(r
W

z
W

0:
w

C,W z

the algorithm described in Appendix B was programmed on digital computer and subsequently tested with various simulated geometries. Representative results for a typical geometry are summarized in Figs. 3 through 6 (additional simulation data also appear in [7] and [18] ). Fig. 3 depicts the target and own ship trajectories, which are identified by the symbols "+" and "0," respectively (these symbols also correspond to 2-min time marks). For convenience each run is terminated after 16 min. In this geometry the initial range is 4400 yd and the initial bearing is 950. The target maintains a steady course of 3200 and a constant speed of 25 knots. Own ship also maintains a constant speed of 6.5 knots, but executes a course change from 30 to 2950 after 8 min into the run. Figs. 4 through 6 show the range, course, and speed errors generated by the pseudolinear filter. To simulate data measurement errors the raw bearings were corrupted by additive zero mean Gaussian noise prior to processing. Three different noise levels were used. The curves labeled A depict TMA solution errors obtained with an rms noise level of 10; those labeled B and C correspond to rms noise levels of 20 and 30, respectively. All results are averaged over 50 Monte Carlo samples, and the appropriate standard deviations are indicated on each graph. The results presented here demonstrate that the pseudoa

linear filter is both a stable and predictable estimation algorithm for bearings-only TMA. Indeed, in all the tests conducted there has been no evidence of abnormal behavior. Furthermore, Figs. 4 through 6 reveal that the estimated solution remains "locked on" to the trival relative motion solution prior to the first own ship maneuver. However, once own ship executes a maneuver the estimated solution begins to converge to the correct TMA solution. As expected, random measurement errors simply delay convergence. This pattern, which was manifest during all tests, is precisely what the theory predicts. In contrast, the extended Kalman filter has experienced various instances of premature covariance collapse and solution divergence under similar test conditions [7]
.

VI. Summary and Conclusions

We have attempted to present a detailed analysis of the extended Kalman filter as applied to bearings-only target tracking. Closed-form expressions for the covariance matrix were introduced and used to demonstrate how bearing estimation errors and/or conventional initialization techniques can precipitate premature covariance collapse. It was further shown that anomalous covariance behavior often causes abrupt variations in the estimated state vector. By utilizing this state vector for subsequent covariance computations (as is done in the extended Kalman filter) an error feedback path is established. The resulting interaction and amplification of estimation errors can then lead to filter divergence. Unfortunately, the extended Kalman filter has no builtin mechanism to insure that consistent estimates are actually
VOL. AES-15, NO. 1 JANUARY 1979

36

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

used during covariance computation. Despite this drawback, the analysis indicates that substantial improvements in filter stability can be realized by taking certain elementary precautions with regard to initialization and relinearization. Some candidate modifications were proposed and discussed in Section IV and these form the basis of the pseudolinear TMA algorithm. Undoubtedly there are other potential remedies which also merit consideration; however, no attempt has been made here to explore all possible alternatives. This is certainly an important area for future research. Although numerous simulation tests were conducted during this study, only a small representative sampling of results have been presented here. Nevertheless, these results are sufficient to demonstrate the efficacy of the pseudolinear filter. In all the tests conducted this algorithm behaved in a completely predictable manner. Most importantly, no evidence of instability has been observed. Accordingly, it is concluded that the pseudolinear filter, as implemented in Appendix B, provides a viable automatic technique for bearings-only TMA.
Appendix A Description of M\odeling Process for Bearings-Only TMA Development of State Equations

VOWt ==
Ao (t) =
LAox(t)]

Vo X(t)

x component of own ship velocity

VOY(t) _j

Ly component of own ship velocityj

Fx component of own ship acceleration

Aoy(t)j

Ly component of own ship acceleration J

Subsequent integration of equation set (Al) yields the familiar expressions

R(t) = R(t*) + (t - t*) V(t*) - fo


(tVt*)

rAo(t - r)dr

(A3a)

(A3b)

where t* denotes any arbitrary fixed value of time. The discrete state equations for bearings-only TMA may now be derived by observing that equation set (A3) is valid for arbitrary values of t and t*. Accordingly, if t = kT and t* = (k - 1)T, it follows that

R(k) =R(k -1) +TV(k -1) -f rAo(kT - -)d'r .(A4a)

Consider the geometry depicted in Fig. 1, with target and (A4b) own ship confined to the same horizontal plane:s If conFinally, multiplying (A4b) by T and rewriting these expresstant target velocity is assumed, the equations of motion sions in matrix notation leads to the desired result; i.e., for this two-dimensional configuration may be written as

V(k) = V(k- 1)-f Ao(kT-r) dT, k = 1, 2, 3, ....

R(t) = V(t) V(t) = -Ao (t)


where

(Ala)

X(k) =A(k, k - 1) X(k - 1)-W(k), k = 1, 2, 3, ... (A5)


RX(k)

(Alb)
X(k) =

Ry(k)

V(t) = Vt(t) -Vo(t)


and

(A2)

JVy(k)_
x component of relative range at time kT y component of relative range at time kT component of relative velocity6 at timekT](A6a) x component of relative velocity6 at time kT Ly component of relative veloCity6 at time kT-

TVx(k)

R(t) ==

FRX(t) 1
F

rx component of relative range]


Ly component of relative range

R RgY(t)

VX(t) 1 rx component of relative velocity]


1y component of relative velocity

L V (t) j L (t)] VV( (t)=

A(k, k- 1)=

0~

Fx component of target velocit]


Kx component of target velocity

0 1 O01010 _O O O 1_

(A6b)

T = sampling period (constant)

(A6c)

this assumption.

'The bearings-only TMA problem is typically formulated under

'Here, the velocity components have been normalized with respect to the sampling period T.
37

AIDALA: KALMAN FILTER BEHAVIOR IN BEARINGS-ONLY TRACKING APPLICATIONS

W(k) =

rf tAox(kT - t) dt fT tA oy(kT - t) dt 0
T fT Aox(kT - t) dt
T fT Aoy(kT -

t) dt

For later use, note that the state transition matrix can be expressed in the general form
-1

E{n(i) n(k)}
(A6d)
Appendix B

= {

a2

(k), j = k
f *k, k =

0,

1, 2,3,

(Al ld)

Description of Pseudolinear Bearings-Only TMVA Algorithm

System Description

A(i, j) =

O
O

O
1

(i -i)
0
1 0

(i - i)
1_

L_O

0 0

i, j = O, 13 2, ...

A mathematical model of the bearings-only TMA process is given by equation sets (AS), (A6), and (Al 1), which appear in Appendix A.
Initialization Phase

(A7)

Furthermore, this matrix has the well-known properties


A(i, j) = A(i, k) A(k, j)

X(010)= [0]
P(O 0) = I = 4 X 4 identity matrix.
Prediction Phase

A-'(i,j)=AU, i).
Development of Measurement Equation

(A8a)

X(klk-1) =A(k, k

1) X(k - 11 k-1)-W(k)

As the name implies, bearings-only target tracking is distinguished by the fact that measured data consists entirely of passive sonar bearings. Consequently, the measurement process is described with a single scalar equation of the form (see Fig. 1)

P(klk-1) =A(k, k-1)P(k - 1l k-1)A'(k, k - 1).


Measurement Phase

(k) = noisy bearing measurement taken at time kT


a2 (k) = variance of measurement noise on j(k)

,B(t) = tan-' [RX(t)/R(t)] + 77(t)

(A9)

where 3(t) is the measured target bearing, and 2(t) represents additive measurement noise. If the pertinent sensors are properly aligned and calibrated, it may be conveniently assumed that 71(t) is a zero mean white noise process with variance G2 (t); i.e.,

H(k)= [cos P(k), -sin P(k), 0, 0].


Correction Phase

G (k) = P(k k-1) H(k) [H(k) P(k l k-1) H'(k)


+

E{77(t)} = 0
E{77(t) 77(t + T)} = { a2(t)
0
r

(AlOa)
0

a2(k)]-f1
G (k) H(k)]

X(kl k) = X(k k-1)-G(k)H(k) X(kl k- 1)


-

*0

(AlOb) P(k l k) = [I

P(k lk- 1).

where E{-} denotes the statistical expectation operator. Since the proceeding results are valid for all values of t, a discrete model of the measurement process can be written as follows:

3(k) =h [X(k)] + X(k)


h [X(k)]
=

(Al la)

Acknowledgment

tan0

'

[R (k)/R (k)]

(Al lb) Guimond, and S.C. Nardone of the Naval Underwater Sys(Allc) work.
tems Center, Newport, RI, for their contributions to this

The author gratefully acknowledges K. Gent, B.W.

E{i7(k)} O
=

38

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. AES-1 5, NO. 1

JANUARY 1979

References

[11 W.H. Foy, "Position-location solutions by Taylor series estimation," IEEE Trans. Aerosp. Electron. Syst., vol. AES-12, Mar. 1976. [2] J.C. Hassab, "Passive tracking of a moving source by a single observer in shaUow water," J. Sound and Vibration, vol. 44, Feb. 1976. [3] J.L. Poirot and G.V. McWilliams, "Navigation by back triangulation," IEEE Trans. Aerosp. Electron. Syst., vol. AES-12, Mar. 1976. [41 R.C. Kolb and F.H. Hollister, "Bearings-only target estimation," Proc. 1st Asilomar Conf: Circuits and Syst., 1967. [51 D.J. Murphy, "Noisy bearings-only target motion analysis," Ph.D. Diss., Dept. Elec. Eng., Northeastern Univ., 1970. [61 J.R. Drugan, "Three dimensional target motion analysis," Librascope Tech. Rep., 1967. [71 V.J. Aidala, "Behavior of the Kalman filter applied to bearings-only target motion analysis," Naval Underwater Systems Center, NUSC Tech. Rep. 4984, Nov. 1976. [81 D.W. Whitcombe, "Pseudo-state measurements applied to recursive nonlinear filtering," Proc. 3rd Symp. Nonlinear Estimation Theory and Its Application, 1972. [91 R.W. Bass et al., "ASW target motion and measurement models," Computer Software Analysis, Inc., Tech. Rep. TR-72-024-01, Sept. 1972 (this document contains an extensive TMA bibliography).

[101 A.H. Jazwinski, Stochastic Processes and Filtering Theory. New York: Academic Press, 1970. [111 S.1. Chou, "Projected surface ship anti-submarine warfare target motion analysis," Naval Undersea Center, NUC Tech. Rep. TN-1717, Nov. 1976. [121 V.J. Aidala and J.S. Davis, "The utilization of data measurement residuals for adaptive Kalman filtering," Proc. IEEE Conf Engineering in the Ocean Environment (Ocean' 73), 1973. [13] R.F. Cline, "Representations for thc generalized inverse of sums of matrices," J. SIAM Numerical A nalvsis, Ser. B, vol. 2, 1965. [141 V.J. Aidala, "Parameter estimation via the Kalman filter," IEEE Trans. Automat. Contr., vol. AC-22, June 1977. [151 A. Albert and R.W. Sittler, "A method for computing least squares estimators that keep up with the data," J. SIAM Contr., Ser. A, vol. 3, 1966. [161 A.E. Hoerl and R.W. Kennard, "Ridge regression: Biased estimation for non-orthogonal problems," Technometrics, vol. 12, 1970. [171 R.E. Bellman and R.E. Kalaba, Nwnerical Inversion of the Laplace Transform. New York: American Elservier,

estimation via bearing observations," IEEE Trans. Aerosp. Electron. Syst., vol. AES-14, July 1978. [19] A.E. Albert and L.A. Gardner, Jr., Stochastic Approximation and Nonlinear Regression. Boston: M.I.T. Press, 1967.

[181

1966. A. G. Lindgren and K.F. Gong, "Position and velocity

Vincent J. Aidala (M 75) was born in Providence, R.l., on February 26, 1942. He received the B.S. degree in applied mathematics and the M.S. degree in electrical engineering from Brown University in 1964 and 1966, respectively. Since 1967 he has been with the Naval Underwater Systems Center, Newport, R.I. His current research interests are in the areas of adaptive estimation and systems identification. Mr. Aidala is a member of Sigma Xi.
A IV.AT A.

KAT.MAN FTITFR1 --- ---1 --- R.ADTMl.(-Tnh . --- RFH A'VIl)D IM

Y TR .

ADPPI AVTKIM. --- . ICATfIfMR

39