Tài liệu Tracking and Kalman filtering made easy P11 pdf

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) 11 GIVENS ORTHONORMAL TRANSFORMATION 11.1 THE TRANSFORMATION The Givens orthonormal transformation for making a matrix upper triangular is made up of successive elementary Givens orthonormal transformations G ; G ; to be defined shortly Consider the matrix T expressed by t 11 t 12 t 13 t 21 t 22 t 23 7 11:1-1ị Tẳ6 t 31 t 32 t 33 t 41 t 42 t 43 First, using the simple Givens orthonormal transformation matrix G the matrix T is transformed to ðt 11 Þ ðt 12 Þ ðt 13 Þ ðt 22 Þ ðt 23 Þ 7 G 1T ẳ 11:1-2ị t 31 t 32 t 33 t 41 t 42 t 43 The transformation G forces the 2,1 term of the matrix T to be zero Now applying another elementary Givens orthonormal transformation G to the above matrix yields ðt 11 Þ ðt 12 Þ ðt 13 Þ ðt 22 Þ ðt 23 ị 7 G 2G 1T ẳ ð11:1-3Þ ðt 32 Þ ðt 33 Þ t 41 t 42 t 43 283 284 GIVENS ORTHONORMAL TRANSFORMATION The second transformation G forces the 3, term to be zero Applying in turn the third Givens orthonormal transformation G to the above matrix now yields ðt 11 Þ ðt 12 Þ ðt 13 Þ ðt 22 Þ ðt 23 Þ 7 ð11:1-4Þ G 3G 2G 1T ẳ t 32 ị ðt 33 Þ ðt 42 Þ ðt 43 Þ Application of these successive elementary Givens orthonormal transformations has forced all the elements of the first column of T to zero below the first element This process is now repeated for the second column of the above matrix with another set of elementary Givens orthonormal transformations so as to force all the elements below the diagonal of the second column to be zero This process is next repeated for the third and last column of the matrix so as to force the elements below the diagonal to be zero, yielding the desired upper triangular matrix expressed by 3 u 11 u 12 u 13 U ¼ u 22 u 23 ð11:1-5Þ 0 u 33 0 0 The first elementary Givens orthonormal transformation G above is given as c1 s1 j 0 s c j 0 7 ð11:1-6Þ G1 ¼ 6 j 0 j where c ¼ cos  ¼ t 11 ỵ t ị 1=2 t 11 21 11:1-6aị and s ẳ sin  ẳ t 21 t 11 ị 1=2 ỵ t 21 ð11:1-6bÞ Examination of (11.1-6) indicates that each of its row matrices (vectors) have unit amplitude and are orthogonal with each other, as should be the case because G is an orthonormal transformation Physically the Givens orthonormal transformation G can be thought of as projecting the column 285 THE TRANSFORMATION vectors of the matrix T onto the row space of G , as discussed in Section 10.2.1; see also Section 4.3 The new space to which G transforms T is such that for the first column matrix of T, designated as t , the new second coordinate t 21 is zero while the first coordinate, t 11 , is not; see (11.1-2) Alternately, we can view the transformation G as rotating the vector t so as to achieve the same result Specifically, we can think of G as rotating the two-dimensional vector formed by the first two coordinates of t such that it lines up with the x axis Equivalently, we can view G as physically performing a rotation of the first two rows of the matrix T so as to make the term t 21 equal zero This is the viewpoint that we shall first take This rotation (called a Givens rotation) is achieved by the upper left-hand corner  matrix of (11.1-6), that is, by the matrix   c1 s1 G 01 ẳ 11:1-7ị s c which acts on the first two rows of the matrix T as follows:  c1 s s1 c1  t 11 t 21 t 12 t 22 t 13 t 23   ẳ t 11 ị ðt 12 Þ ðt 22 Þ ðt 13 Þ ðt 23 Þ  ð11:1-8Þ Using (11.1-6a) and (11.1-6b), it is easy to verify that t 11 ị ẳ c t 11 ỵ s t 21 2 1=2 ¼ ðt 11 þ t 21 Þ ð11:1-9Þ and that the lower left-hand component of the matrix on the right-hand side of (11.1-8) above is zero, that is, t 21 ị ẳ s t 11 ỵ c t 21 ẳ0 ð11:1-10Þ The rotation performed by the matrix G 01 of (11.1-7) is illustrated in Figure 11.1-1 Consider the vector t ẳ t 11 i ỵ t 21 j 11:1-11ị formed by the first two coordinates of the first column matrix t of T Here i and j are unit direction vectors for respectively the first and second coordinates of t , the x and y coordinates respectively Figure 11.1-1a shows these vectors The matrix G 01 rotates the vector t through an angle  defined by (11.1-6a) or (11.1-6b) to form a new vector t having the same magnitude but now aligned 286 GIVENS ORTHONORMAL TRANSFORMATION Figure 11.1-1 (a) Givens rotation of vector t in x–y plane onto x axis forming vector t (b) Two Givens rotations of three-dimensional vector t to form vector t aligned along x axis 287 THE TRANSFORMATION along the x axis so as to be given by 2 1=2 t ¼ t 11 ỵ t 21 ị i ỵ 0j 11:1-12ị The second Givens orthonormal transformation G of (11.1-3) is given as c2 s2 07 11:1-13ị G2 ẳ s c 0 where c ¼ cos  ¼ ðt 11 ị 1=2 ẵt 11 ị 21 ỵ t 31 11:1-13aị and s ẳ sin  ẳ t 31 ẵt 11 ị 21 1=2 ỵ t 31 ð11:1-13bÞ This Givens rotation yields, for the top left-hand corner element of the transformed matrix, 1=2 ðt 11 ị ẳ ẵt 11 ị 21 ỵ t 31 2 1=2 ẳ ẵt 11 ỵ t 21 þ t 31 ð11:1-14Þ and, for the third element in the first column, t 31 ị ẳ 11:1-15ị Figure 11.1-1b illustrates the first rotation by  and the second rotation by the amount  Here the three-dimensional vector t is given by t ẳ t ỵ kt 31 ẳ it 11 ỵ jt 21 ỵ kt 31 11:1-16ị where k is the unit vector along the z axis This vector is first rotated by G onto the x–z plane to form t ỵ kt 13 and then rotated by G onto the x axis to yield the new vector t : 2 1=2 t ¼ ẵt 11 ỵ t 21 ỵ t 31 i 11:1-17ị 288 GIVENS ORTHONORMAL TRANSFORMATION To get further physical insight into the Givens transformation G , we will now view it as transforming the column space of the matrix T onto the new space defined by the row vectors of the matrix G We first consider the projection of the first column vector t of T onto the row space of G (A word is in order relative to our notation Note that whereas t is the whole s-dimensional first column of T, the vector t consists of only the first two coordinates of t The vector t is just t rotated onto the x axis while the vector t is a vector formed by just the first three coordinates of t ) We will consider the projection of t onto the successive rows of G , starting with the first, in order to obtain the coordinates of t after projecting it into the row space of G Designate the first row of G as the vector g whose coordinates (in the original x, y, z, coordinate system; see Section 4.3) are given by the entries of the first row of G This vector g is a unit vector lying in the plane defined by the first two coordinates of the column vector t of T This plane is the x, y space of Figure 11.1-2a Moreover, g is chosen to lie along the vector t formed by the first two coordinates of t with the remaining coordinates being zero Hence, from Figure 11.1-2a and (11.1-6) g ¼ i cos  ỵ j sin  ỵ k þ þ ð11:1-18Þ where i, j, and k are the unit vectors in the x, y, and z directions of the rectangular coordinate system (see Figure 11.1-2c) They are also the unit vector directions for the vector formed by the first three coordinates of the column vectors of T, that is, of the vector t Figure 11.1-2 (a) Transformation of vector formed by first two coordinates of t , designated T , into g , g two-dimensional space from x; y space (b) Transformation of vector formed by t ¼ g k t k and kt 31 (effectively first three coordinates of t ), designated as t , into (g Þ , ðg Þ space (c) Givens unit vectors 289 THE TRANSFORMATION Figure 11.1-2 (Continued) The projection of t onto g is given by the dot product of t and g [see (4.2-36)], or equivalently by the product of the first row of G by the first column of T as given by (4.2-37) for the three-dimensional case Because only the x, y coordinates of g are nonzero, only the first two coordinates of t enter into this product, or equivalent, projection Thus we can think of g as a two element vector to get g t ẳ g t 11:1-19ị 290 GIVENS ORTHONORMAL TRANSFORMATION Applying (11.1-6a) and (11.1-6b) to t of (11.1-11) yields t ¼ ik t k cos  ỵ jk t k sin  ð11:1-20Þ Substituting (11.1-18) and (11.1-20) into (11.1-19) and using (11.1-9) yield g t ¼ g t ẳ kt k ẳ t 11 ị ð11:1-21Þ That is, the projection of t into g is equal to the magnitude of t , as we saw it should be from the above discussion relative to thinking of G as doing a rotation of t This result also follows because, as indicated above, the unit vector g was chosen to lie not only in the x, y plane but also along the vector t ; see (11.1-18) and (11.1-20) Equation (11.1-21) gives us the first coordinate of the projection of t onto the s-dimensional row space of G Now let us determine the second coordinate obtained by projecting t onto the second-row unit vector of G , which we designate as g The vector g is also in the x, y plane of Figure 11.1-2a but perpendicular to g Specifically, from Figure 11.1-2a and (11.1-6) it follows that g ẳ i sin  ỵ j cos  ỵ k ỵ ỵ 11:1-22ị The projection of t onto g is given by g t ¼ g t ð11:1-23Þ and substituting (11.1-20) and (11.1-22) into the above equation yields g t ¼ sin  cos  ỵ cos  sin  ẳ ð11:1-24Þ which, based on (11.1-2) and (11.1-8), is what it should be Physically (11.1-24) should be zero because g is orthogonal to the direction of g1 , which is lined up with the direction of t and in turn t Equation (11.1-24) gives us the second coordinate of the projection of t1 onto the s-dimensional space of G Now we determine the third coordinate by projecting t onto the third-row unit vector of G , which we designate as g Examining (11.1-6), we see that the third row consists of zero entries except for the third-column entry, which is a Consequently, the projection of t1 onto the third-row unit vector g3 leaves the third coordinate of t1 unchanged Stated another way, g is a unit vector aligned along the third coordinate of t1 This follows because all the coordinates of g are zero except the third, which is unity Consequently, projecting t1 onto g leaves the third coordinate of t1 unchanged The same is true for the remaining coordinates of t1 ; they are unchanged by the transformation G because the ith-row vector g i, i > 2, is a THE TRANSFORMATION 291 unit vector lined up with the ith coordinate of t1 ; see (11.1-6) Although for (11.1-6) s ¼ 4, this result is true when G is an s  s matrix In this case, the lower right-hand corner ðs  2Þ  ðs  2Þ matrix of G is the identity matrix with the elements to its left being zero Thus Figure 11.1-2a illustrates the projection of the vector t 0, defined by the first two coordinates of t , onto the space defined by the first two unit row vectors g and g of G This completes the proof that G projects T into the form given by (11.1-2) Designate this projected vector t as ðt Þ In summary, G transforms the columns of T to a new coordinate system for which the coordinates of T have the form given by (11.1-2) with ðt 21 Þ ẳ We will let t ị represent the column vector t 1, the first column of T, in this new coordinate system Let us carry one step further our discussion of viewing the simple G i Givens transformations as projecting the column space of the matrix T onto the space defined by the row vectors of G i This is now done by detailing this projection for G On examining (11.1-13) it is seen that only the first and third coordinates are altered by this projection, the unit vectors of G represented by the other rows of G being unit vectors aligned along the other coordinates of the matrix T Now consider the unit vectors ðg Þ and ðg Þ defined by the first and third rows of G On examining (11.1-16), (11.1-13a), (11.1-13b), and (11.1-13), it can be verified that ðg Þ is chosen to line up with the vector defined by the first- and third-row coordinates of ðt Þ , with all other coordinates set equal to zero, while ðg Þ is chosen to be orthogonal to ðg Þ This situation is illustrated in Figure 11.1-2b; see also Figure 11.1-2c In this figure t is the vector formed by the first- and third-row coordinates of ðt Þ ; see (11.1-2) and (11.1-16) As a consequence of this choice for ðg Þ 2, the projection of t onto ðg Þ , which is the first coordinate of the vector G ðt Þ 1, has a magnitude equal to the magnitude of t , which is the desired ðt 11 Þ of (11.1-3); see also (11.1-14) In turn t equals the magnitude of the first three coordinates of t ; see (11.1-14) Moreover, this last result follows because ðg Þ in the above is actually chosen to line up with the first three coordinates of the vector t Because ðg Þ is orthogonal to ðg Þ and in turn t , the third coordinate of G ðt Þ given by ðg Þ t ị ẳ g ị 2t 2, equals zero This results in the first column of G G T being in the form given in (11.1-3), as desired Figure 11.1-2b illustrates the projection of the vector t 2, defined by the vectors t and kt 31 , onto the space defined by the first and third rows of G , or equivalently, the unit vector rows ðg Þ and ðg Þ of G The physical significance of the succeeding Givens transformations follows in a similar manner This completes our interpretation of the Givens transformations as an orthogonal coordinate transformation In Chapter 13 we shall relate the Givens transformation to the Gram–Schmidt transformation introduced in Section 4.3 In Chapter 12 we shall relate the Givens transformation to the Householder transformation 292 GIVENS ORTHONORMAL TRANSFORMATION The transformations can be conveniently applied to T and Y ðnÞ simultaneously by generating the augmented matrix T given by T ẳ ẵTjY nị 11:1-25ị and then applying the Givens orthonormal transformation to it [This parallels the procedure followed in Section 4.3 where such an augmented matrix was generated to apply the Gram–Schmidt transformation to T and Y ðnÞ simultaneously; see (4.3-54) and the discussions related to it] To be concrete, assume that T is a  matrix given by t 11 t 12 ð11:1-26Þ T ¼ t 21 t 22 t 31 t 32 and Y nị y1 ẳ y2 y3 ð11:1-27Þ   x1 x ð11:1-28Þ Also assume ¼ X n;n Then (11.1-25) becomes t 11 T ¼ t 21 t 31 y1 y2 y3 t 12 t 22 t 32 ð11:1-29Þ After three Givens rotations (11.1-29) becomes ðt 11 Þ ðt 12 Þ ðt 22 Þ T 00 ¼ FT ¼ -0 j j j j ðy Þ ðy Þ 7 ðy Þ ð11:1-30Þ As shall be shown shortly, the square of the lower right element ðy Þ is the numerical value of the least-squares residue error given by (4.1-31), that is, ị ẵy ị ¼ ðy Þ 23 ¼ eðX n;n ð11:1-31Þ [This result was already shown in Section 4.3; specifically see (4.3-38).] The  upper left-hand corner matrix of t’s corresponds to the upper triangular 300 GIVENS ORTHONORMAL TRANSFORMATION The above matrix multiplication can be carried out with the systolic array shown in Figure 11.3-1 Each square of the systolic array does a multiplication and addition as indicated The outputs y i , i= 1, 2, 3, are generated as the outputs of the rightmost box The inputs for a given column of the x ij are shown staggered in the figure This is necessary because the output of the box labeled a i1 is needed before the calculations for the box labeled a i can be carried out To better understand what is happening, let us follow the computation of y First a x 11 is computed at the first clock cycle in the box labeled a Then a x 11 ỵ a x 12 is calculated on the next clock cycle in the box labeled a Finally, the desired output a x 11 ỵ a x 12 þ a x 13 ¼ y is computed on the third clock cycle in the box labeled a While y is being calculated, y and y are being calculated behind it Figure 11.3-2 (a) Systolic array implementation of Givens orthonormal transformation (b) Computations performed by circular boundary and rectangular internal cells of systolic array implementing Givens orthonormal transformation (After Kung, H T and W M Gentleman, ‘‘Matrix Triangularization by Systolic Arrays,’’ Proceedings of the SPIE, 1981, Vol 298, Real Time Signal Processing IV.) SYSTOLIC ARRAY IMPLEMENTATION 301 Now we will define a systolic array It is a collection of individual computer boxes or elements, many of which identical computations These elements calculations in parallel and most importantly have only a connection to adjacent computer boxes, or elements, as is the case for the systolic array of Figure 11.3-2 A key feature of the systolic array is that the computations are carried out in parallel by many computer processors, thus providing a high throughput The computer elements making up the systolic array could be arranged as a one-dimensional array, as in Figure 11.3-1, a two-dimensional array (as is the case for the systolic array needed to the Givens orthonormalization), or a three-dimensional array We now give the two-dimensional systolic array needed to the Givens orthonormal transformation The systolic array required to the Givens orthonormal transformation is given in Figure 11.3-2 [106] It consists of two types of computer elements, those in the square boxes and those in the circular boxes The circular elements compute the c i ’s and s i ’s of (11.1-6a) and (11.1-6b) needed for the Givens rotation, the ith-row circular box calculating the c i ’s and s i ’s for the ith row of T Assume T is a ðm þ 1Þ  s matrix Then, except for the bottom circular box, the circular boxes calculate elements ðt ii Þ k The bottom circular box calculates y mỵ1 ị k , where k ¼ 1; 2; ; s and s is the number of Givens Figure 11.3-2 (Continued) 302 GIVENS ORTHONORMAL TRANSFORMATION rotations needed to complete the orthonormal transformation to obtain T 00 ¼ FT ; see, for example, (11.1-30) At the end of the computations the circular boxes contain ðt ii Þ s ¼ u ii i ¼ 1; ; m ð11:3-2Þ and the bottom circular box contains ị 1=2 y mỵ1 ị s ẳ ẵe n ðX n;n ð11:3-3Þ The inner square boxes calculate ðt ij ị k for i 6ẳ j At the completion of the triangularization the inner square boxes contain ðt ij Þ s ¼ u ij for i 6¼ j ð11:3-4Þ The right-hand boundary square boxes calculate ðy i Þ k and at the end of the Givens orthonormal transformation contain y i ị s i ẳ 1; ; m ð11:3-5Þ Figure 11.3-3 shows the step-by-step computations involved in the Givens orthonormalization of T given by (11.2-1) for " ¼ 0:1 At the end of the orthonormalization the array contains the orthonormalized T given by (11.2-2) Having performed the Givens orthonormal transformation of T using the systolic array, it is now possible to calculate the desired least-squares estimate ; which is an m  array containing the elements x ; ; x This is done X n;n m using the back-substitution method to solve (10.2-16) as described in Sections 4.3 and 10.2 This back substitution can also be carried out using another systolic array [107; see also 106] Cascading these two systolic arrays together yields the systolic array of Figure 11.3-4 [106] In the above systolic array implementation the circular elements have more complex computations than the square elements The circular elements require a square-root operation [see (11.1-6a) and (11.1-6b)] whereas the square elements only require multiplication and add operations A square-root free implementation of the Givens rotation algorithm has been developed [106, 108, 109] It is only when this square-root free algorithm is implemented that the diagonal connections between the circular elements indicated in Figure 11.3-2 are needed For the square-root algorithm given above they are not needed [106; see Figure 11.3-2b] For the sidelobe canceler problem of Section 4.4 we are interested in calculating the least-squares residual and not the weight vector McWhirter [89, 94] has developed an efficient triangular systolic array (like that of Figure 11.3-2) which computes the residue without the need for the back-substitution systolic array of Figure 11.3-4 The residual is obtained as the output of the bottom circular element of the systolic array Both square-root and square-root free ... Using (11.1-6a) and (11.1-6b), it is easy to verify that t 11 ị ẳ c t 11 þ s t 21 2 1=2 ¼ ðt 11 ỵ t 21 ị 11:1-9ị and that the lower left-hand component of the matrix on the right-hand side of (11.1-8)... coordinates of the first column matrix t of T Here i and j are unit direction vectors for respectively the first and second coordinates of t , the x and y coordinates respectively Figure 11.1-1a shows... coordinates being zero Hence, from Figure 11.1-2a and (11.1-6) g ¼ i cos  ỵ j sin  ỵ k þ þ ð11:1-18Þ where i, j, and k are the unit vectors in the x, y, and z directions of the rectangular coordinate