Tài liệu Tracking and Kalman filtering made easy P14 pptx

The comparison includes the computer requirements needed when using the normal equations given by 4.1-30 with the optimum least-squares weight W given by 4.1-32.. Before leaving this sec

MORE ON VOLTAGE-PROCESSING TECHNIQUES

14.1 COMPARISON OF DIFFERENT VOLTAGE

LEAST-SQUARES ALGORITHM TECHNIQUES

Table 14.1-1 gives a comparison for the computer requirements for the different voltage techniques discussed in the previous chapter The comparison includes the computer requirements needed when using the normal equations given by (4.1-30) with the optimum least-squares weight W given by (4.1-32) Table 14.1-1 indicates that the normal equation requires the smallest number of computations (at least when s > m, the case of interest), followed by the Householder orthonormalization, then by the modified Gram–Schmidt, and finally the Givens orthogonalization However, the Givens algorithm computa-tion count does not assume the use of the efficient CORDIC algorithm The assumption is made that all the elements of the augmented matrix T0 are real When complex data is being dealt with, then the counts given will be somewhat higher, a complex multiply requiring four real multiplies and two real adds, a complex add requiring two real adds The Householder algorithm has a slight advantage over the Givens and modified Gram–Schmidt algorithms relative to computer accuracy Table 14.1-2 gives a summary of the comparison

of the voltage least-squares estimation algorithms

Before leaving this section, another useful least-squares estimate example is given showing a comparison of the poor results obtained using the normal equations and the excellent results obtained using the modified Gram–Schmidt algorithm For this example (obtained from reference 82)

339

Tracking and Kalman Filtering Made Easy Eli Brookner

Copyright # 1998 John Wiley & Sons, Inc ISBNs: 0-471-18407-1 (Hardback); 0-471-22419-7 (Electronic)

1 1 1

" 0 0

0 " 0

0 0 "

2 6 6 4

3 7 7

YðnÞ¼ Yð4Þ¼

1 0 0 0

2 6 6 4

3 7 7

for (4.1-11a) We now slove for the least-squares estimate using the normal equations given by (4.1-30) and (4.1-32) We shall at first obtain the exact solution Toward this end, we calculate

TTT¼

1þ "2 1 1

1 1þ "2 1

1 1 1þ "2

2 4

3

and

TTYð4Þ¼

1 1 1

2 4

3

Then from (4.1-30) and (4.1-32) it follows that the exact solution (no round-off errors) is

X n;n ¼ 1

3þ "2

1 1 1

2 4

3

TABLE 14.1-1 Operation Counts for Various Least-Squares Computational Methods

Asymptotic Number

Normal equations (power method) 12sm2þ1

6m3

3m3

3m3

Note: The matrix T is assumed to be an s  m matrix.

Source: From references 80 and 81.

a An operation is a multiply, or divide, plus and add.

Now we obtain the normal equation solution Assume eight-digit floating-point arithmetic If "¼ 104, then 1þ "2¼ 1:00000001, which is rounded off to 1.0000000 The matrix TTT then is thought to contain all 1’s for its entries and becomes singular and noninvertable so that no least-squares estimate is obtainable using the normal equations (4.1-30) and (4.1-32)

Next let us apply the modified Gram–Schmidt algorithm to this same example The same eight-digit floating-point arithmetic is assumed It follows that Q0 of (13.1-20) becomes

Q0¼

" " 1

2" 1

3"

0 " 1

2" 1

3"

0 " " 1"

2 6 6

3 7

TABLE 14.1-2 Voltage Least-Squares Estimate Algorithms

(Orthonormal Transforms): Trade-offs

Householder

Lowest cost on a serial (nonparallel, single-central-processor) machine

Best numerical behavior (by a small margin)

Givens Rotations

Introduces one zero at a time and as a result is more costly in number of

computations required

However, allows parallel implementations: linear and triangular systolic arrays Rotations can be efficiently implemented in hardware using CORDIC

number representations

Square-root free version requires computations equal to that of Householder but is

no longer orthogonal and requires large dynamic range (generally alright if floating point used)

Modified Gram–Schmidt

Like Givens MGS is more costly than Householder

Like Givens is amenable to systolic implementation

Provides joint order/time recursive updates

General Comment: Where accuracy is an issue, the orthonormal transforms

(voltage methods) are the algorithms of choice over the normal equation

With the development of microcomputers having high precision, like 32 and 64 bits floating point, accuracy is less of an issue Where a high throughput is needed, the systolic architecture offered by the Givens approach can provide the high

throughput of parallel processing

Source: After Steinhardt [138].

DIFFERENT VOLTAGE LEAST-SQUARES ALGORITHM TECHNIQUES 341

and U0 and Y100of (13.1-39) becomes

U0¼

1 1 1

0 1 1 2

0 0 1

2 4

3

and

Y100¼

1 1 2 1 3

2 4

3

Finally substituting the above in (13.1-42) and solving using the back-substitution method yields

X n;n ¼1 3

1 1 1

2 4

3

which is close to the exact solution of (14.1-5)

Those who desire to further pursue the voltage techniques described are urged to read references 76, 79, 81 to 83, 89, 91, 101 to 103, 115, 118 to 122, and 139 References 79, 115, 119, 121, and 122 apply the voltage techniques to the Kalman filter; see Section 14.5

14.2 QR DECOMPOSITION

In the literature what is called the QR decomposition method is described for solving the least-squares estimation problem [81, 89, 102] This involves the decomposition of the augmented matrix T0 into a product of matrices designated as QR as done in (13.1-27) when carrying out the Gram–Schmidt algorithm in Chapter 13 Thus the Gram–Schmidt is a QR decomposition method It follows that the same is true for the Householder and Givens methods These give rise to the upper triangular R when the augmented T0 is multiplied by an orthogonal transformation QT, that is, QTT0¼ R; see, for example (11.1-30) or (12.2-6), where QT ¼ F for these equations Thus from (10.2-1), QQTT0 ¼ T0 ¼ QR, and the augmented matrix takes the form QR, as desired

An additional physical interpretation of the matrices Q and R can be given that is worthwhile for obtaining further insight into this decomposition of the augmented matrix T0 When Q is orthonormal, its magnitude is in effect unity, and it can be thought of as containing the phase information of the augmented matrix T The matrix R then contains the amplitude information of T The QR

can then be thought of as the polar decompostion of the augmented matrix T0 into its amplitude R and phase Q components That this is true can be rigorously proven by the use of Hilbert space [104]

14.3 SEQUENTIAL SQUARE-ROOT (RECURSIVE)

PROCESSING

Consider the example augmented matrix T0 given by (11.1-29) In this matrix time is represented by the first subscript i of two subscripts of the elements tij, and the only subscript i of yi Thus the first row represents the observation obtained first, at time i¼ 1, and the bottom row the measurement made last For convenience we shall reverse the order of the rows so that the most recent measurement is contained in the top row Then without any loss of information (11.1-29) becomes

T0¼

t31 t32 y3

t21 t22 y2

t11 t12 y1

2 4

3

To solve for the least-squares estimate, we multiply the above augmented matrix T0by an orthonormal transformtion matrix to obtain the upper triangular matrix given by

T00 ¼

ðt31Þ3 ðt32Þ3 j ðy3Þ3

0 ðt22Þ3 j ðy2Þ3 - - j

-0 0 j ðy1Þ3

2 6 4

3 7

The entries in this matrix differ from those of (11.1-30) because of the reordering done Using the back-substitution method, we can solve for the least-squares estimate by applying (14.3-2) to (10.2-16)

Assume now that we have obtained a new measurement at time i¼ 4 Then (14.3-1) becomes

T0¼

t41 t42 y4

t31 t32 y3

t21 t22 y2

t11 t12 y1

2 6 4

3 7

We now want to solve for the least-squares estimate based on the use of all four measurements obtained at i¼ 1; 2; 3; 4 A straightforward method to obtain the least-squares would be to repeat the process carried out for (14.3-1), that is, triangularize (14.3-3) by applying an orthonormal transformation and then use

SEQUENTIAL SQUARE-ROOT (RECURSIVE) PROCESSING 343

the back-substitution procedure This method has the disadvantage, however, of not making any use of the computations made previously at time i¼ 3

To make use of the previous computations, one adds the new measurements obtained at time i¼ 4 to (14.3-2) instead of (14.3-1) to obtain the augmented matrix [79]

T00 ¼

t41 t42 y4

ðt31Þ3 ðt32Þ3 ðy3Þ3

0 ðt22Þ3 ðy2Þ3

0 0 ðy1Þ3

2 6 6

3 7

One can now apply an orthonormal transformation to (14.3-4) to upper triangularize it and use the back substitution to obtain the least-squares estimate This estimate will be based on all the measurements at i¼ 1; 2; 3; 4 The computations required when starting with (14.3-4) would be less than when starting with (14.3-3) because of the larger number of zero entries in the former matrix This is readily apparent when the Givens procedure is used Furthermore the systolic array implementation for the Givens algorithm as represented by Figures 11.3-2 and 11.3-4 is well suited to implementing the sequential algorithm described above

The sequential procedure outlined above would only be used if the least-squares estimate solutions are needed at the intermediate times

i¼ 1; 2; 3; 4; ; j; ; etc This is typically the case for the radar filtering problem If we did not need these intermediate estimates, it is more efficient to only obtain the estimate at time i¼ s at which the last measurement is made [79] This would be done by only triangularizing the final augmented matrix T0 containing all the measurements from i¼ 1; ; s

Sometimes one desires a discounted least-squares estimate: see Section 1.2.6 and Chapter 7 To obtain such an estimate using the above sequential procedure, one multiplies all the elements of the upper triangularized matrix T0by  where

0 <  < 1 before augmenting T0 to include the new measurement as done in (14.3-4) Thus (14.3-4) becomes instead

T0 ¼

t41 t42 y4

ðt31Þ3 ðt32Þ3 ðy3Þ3

0 ðt22Þ3 ðy2Þ3

0 0 ðy1Þ3

2 6 4

3 7

The multiplication by  is done at each update Such a discounted (or equivalently weighted) least-squares estimate is obtained using the systolic arrays of Figures 11.3-2 and 11.3-4 by multiplying the elements of the systolic array by  at each update; see references 83 and 89

The sequential method described above is sometimes referred to in the literature as the sequential square-root information filter (SRIF) [79] The reader is referred to reference 79 for the application of the sequential SRIF to

the Kalman filter The reader is also referred to references 78, 81 to 83, 89, 102, and 119

14.4 EQUIVALENCE BETWEEN VOLTAGE-PROCESSING

METHODS AND DISCRETE ORTHOGONAL LEGENDRE

POLYNOMIAL APPROACH

Here we will show that the voltage-processing least-square approach of Section 4.3 and Chapters 10 to 13 becomes the DOLP approach of Section 5.3 when a polynomial of degree m is being fitted to the data, that is, when the target motion as a function of time is assumed to be described by a polynomial of degree m, and when the times between measurements are equal Consider again the case where only range xðrÞ ¼ xris measured Assume, [see (5.2-3)], that the range trajectory can be approximated by the polynomial of degree m

xðrÞ ¼ xr¼ pðtÞ ¼Xm

j¼0

ajðrTÞj ð14:4-1Þ

where for simplicity in notation we dropped the bar over the aj Alternately from (5.2-4)

xðrÞ ¼ xr¼ pðtÞ ¼Xm

j¼0

where

where zj is the scaled jth state derivative; see (5.2-8) The values of

xr; r¼ 0; ; s, can be represented by the column matrix

X¼

x0

x1

xr

xs

2 6 6 6 6 4

3 7 7 7 7 5

ð14:4-3Þ

Note that the column matrix X defined above is different from the column matrix used up until now The X defined above is physically the matrix of the true ranges at times r¼ 0; 1; 2; ; s It is the range measurements yr made without the measurement noise error, for example, Nn of (4.1-1) or NðnÞ of

EQUIVALENCE BETWEEN VOLTAGE-PROCESSING METHODS 345

(4.1-11a) The X defined up until now was the m0 1 column matrix of the process state vector When this state vector X is multiplied by the observation matrix M or the transition–observation matrix T, it produces the range matrix X defined by (14.4-3) This is the only section where we will use X as defined by (14.4-3) Which X we are talking about will be clear from the text Which of these two X’s is being used will also be clear because the state vector X n always has the lowercase subscript s or n or 3 The range matrix will usually not have a subscript or will have the capital subscript Z or B as we shall see shortly Applying (14.4-2) to (14.4-3) yields

X¼

x0

x1

x2

xr

xs

2

6

6

6

6

6

6

4

3 7 7 7 7 7 7 5

¼

z0 þ 0 þ þ 0

z0 þ z1 þ þ zm1m

z0 þ z12 þ þ zm2m

z0 þ z1r þ þ zmrm

z0 þ z1s þ þ zmsm

2 6 6 6 6 6 6 4

3 7 7 7 7 7 7 5

ð14:4-4Þ

or

where

Zs¼

z0

z1

z2

zm

2 6 6 6 4

3 7 7 7 5

ð14:4-5aÞ

and

T ¼

1 0 0 0

10 11 12 1m

20 21 22 2m

r0 r1 r2 rm

s0 s1 s2 sm

2

6

6

6

6

6

4

3 7 7 7 7 7 5

¼

1 0 0 0

1 1 1 1

1 21 22 2m

1 r r2 rm

1 s s2 sm

2 6 6 6 6 6 4

3 7 7 7 7 7 5 ð14:4-5bÞ

Physically Zsis the scaled state matrix Znof (5.4-12) for index time s instead of n; see also (5.2-8) and (14.4-2a) Here, T is the transition–observation matrix for the scaled state matrix Z

Physically, (14.4-5) is the same as (4.1-11) when no noise measurements errors are present, that is, when NðnÞ¼ 0 As indicated, Zsis scaled version of the state matrix Xn of (4.1-11) or (4.1-2) and the matrix T of (14.4-5) is the transition–observation matrix of (4.1-11) or (4.1-11b) for the scaled state matrix

Zs, which is different from the transition–observation matrix T for Xs The range xðrÞ trajectory is modeled by a polynomial of degree m, hence Xsand in turn its scaled form Zshave a dimension m0 1 where m0¼ m þ 1; see (4.1-2) and discussion immediately following

Now from (5.3-1) we know that xðrÞ can be expressed in terms of DOLP

jðrÞ, j ¼ 0; ; m as

xðrÞ ¼ xr ¼Xm

j¼0

jjðrÞ ð14:4-6Þ

where for simplicity we have dropped the subscript n on j In turn, using (14.4-6), the column matrix of xr, r¼ 0; ; s, can be written as

X¼

x0

x1

x2

xr

xs

2

6

6

6

6

6

6

4

3 7 7 7 7 7 7 5

¼

0ð0Þ 1ð0Þ mð0Þ

0ð1Þ 1ð1Þ mð0Þ

0ð2Þ 1ð2Þ mð2Þ

0ðrÞ 1ðrÞ mðrÞ

0ðsÞ 1ðsÞ mðsÞ

2 6 6 6 6 6 6 4

3 7 7 7 7 7 7 5

0

1

2

r

m

2 6 6 6 6 6 6 4

3 7 7 7 7 7 7 5

ð14:4-7Þ

or

where

P¼

0ð0Þ 1ð0Þ mð0Þ

0ð1Þ 1ð1Þ mð1Þ

0ð2Þ 1ð2Þ mð2Þ

0ðrÞ 1ðrÞ mðrÞ

 ðsÞ  ðsÞ  ðsÞ

2 6 6 6 6 6 6 6

3 7 7 7 7 7 7 7

ð14:4-8aÞ

EQUIVALENCE BETWEEN VOLTAGE-PROCESSING METHODS 347

B¼

0

1

r

m

2 6 6 6 6 6

3 7 7 7 7 7

ð14:4-8bÞ

The DOLP jðrÞ can be written as

2ðrÞ ¼ c20þ c21rþ c22r2 ð14:4-9cÞ

mðrÞ ¼ cm0þ cm1rþ cm2r2þ þ cmmrm ð14:4-9dÞ where the coefficients cijcan be obtained from the equations of Section 5.3 In matrix form (14.4-9a) to (14.4-9d) is expressed by

where T is defined by (14.4-5b) and C is given by the upper triangular matrix of DOLP coefficients:

C¼

c00 c10 c20 cm1;0 cm0

0 c11 c21 cm1;1 cm1

0 0 c22 cm1;2 cm2

0 0 0 cm1;m1 cm;m1

2

6

6

6

6

6

4

3 7 7 7 7 7 5

ð14:4-10aÞ

Substituting (14.4-10) into (14.4-8) yields

for the range matrix X in terms of the DOLP Now xris the actual range What

we measure is the noise-corrupted range yr, r¼ 0; 1; 2; ; s, given by

as in (4.1-1) to (4.1-1c) In matrix form this becomes