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

Applying in turnthe third Givens orthonormal transformation G3to the above matrix now yields37 Application of these successive elementary Givens orthonormal tions has forced all the elem

GIVENS ORTHONORMAL

TRANSFORMATION

The Givens orthonormal transformation for making a matrix upper triangular

is made up of successive elementary Givens orthonormal transformations

G1; G2; to be defined shortly Consider the matrix T expressed by

37

37

The transformation G1 forces the 2,1 term of the matrix T to be zero Nowapplying another elementary Givens orthonormal transformation G2 to theabove matrix yields

37

283

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)

The second transformation G2 forces the 3, 1 term to be zero Applying in turnthe third Givens orthonormal transformation G3to the above matrix now yields

37

Application of these successive elementary Givens orthonormal tions has forced all the elements of the first column of T to zero below the firstelement This process is now repeated for the second column of the abovematrix with another set of elementary Givens orthonormal transformations so as

transforma-to force all the elements below the diagonal of the second column transforma-to be zero.This process is next repeated for the third and last column of the matrix so as toforce the elements below the diagonal to be zero, yielding the desired uppertriangular matrix expressed by

U 0

24

37

377

vectors of the matrix T onto the row space of G1, as discussed in Section 10.2.1;see also Section 4.3 The new space to which G1 transforms T is such that forthe first column matrix of T, designated as t1, the new second coordinate t21iszero while the first coordinate, t11, is not; see (11.1-2) Alternately, we can viewthe transformation G1 as rotating the vector t1 so as to achieve the same result.Specifically, we can think of G1 as rotating the two-dimensional vector formed

by the first two coordinates of t1 such that it lines up with the x axis.Equivalently, we can view G1 as physically performing a rotation of the firsttwo rows of the matrix T so as to make the term t21 equal zero This is theviewpoint that we shall first take This rotation (called a Givens rotation) isachieved by the upper left-hand corner 2 2 matrix of (11.1-6), that is, by thematrix

G01¼ c1 s1

s1 c1

ð11:1-7Þwhich acts on the first two rows of the matrix T as follows:

formed by the first two coordinates of the first column matrix t1of T Here i and

j are unit direction vectors for respectively the first and second coordinates of t1,the x and y coordinates respectively Figure 11.1-1a shows these vectors Thematrix G01 rotates the vector t0 through an angle 1 defined by (11.1-6a) or(11.1-6b) to form a new vector t having the same magnitude but now aligned

Figure 11.1-1 (a) Givens rotation of vector t0in x–y plane onto x axis forming vector

along x axis

along the x axis so as to be given by

37

¼ ½t211þ t221þ t312 1=2 ð11:1-14Þand, for the third element in the first column,

To get further physical insight into the Givens transformation G1, we willnow view it as transforming the column space of the matrix T onto the newspace defined by the row vectors of the matrix G1 We first consider theprojection of the first column vector t1of T onto the row space of G1 (A word is

in order relative to our notation Note that whereas t1 is the whole sional first column of T, the vector t0consists of only the first two coordinates of

s-dimen-t1 The vector t1 is just t0 rotated onto the x axis while the vector t2is a vectorformed by just the first three coordinates of t1.) We will consider the projection

of t1onto the successive rows of G1, starting with the first, in order to obtain thecoordinates of t1 after projecting it into the row space of G1

Designate the first row of G1 as the vector g1 whose coordinates (in theoriginal x, y, z, coordinate system; see Section 4.3) are given by the entries ofthe first row of G1 This vector g1is a unit vector lying in the plane defined bythe first two coordinates of the column vector t1of T This plane is the x, y space

of Figure 11.1-2a Moreover, g1 is chosen to lie along the vector t0 formed bythe first two coordinates of t1 with the remaining coordinates being zero.Hence, from Figure 11.1-2a and (11.1-6)

g1¼ i cos 1þ j sin 1þ k 0 þ 0 þ ð11:1-18Þwhere i, j, and k are the unit vectors in the x, y, and z directions of therectangular coordinate system (see Figure 11.1-2c) They are also the unitvector directions for the vector formed by the first three coordinates of thecolumn vectors of T, that is, of the vector t1

The projection of t1 onto g1 is given by the dot product of t1 and g1 [see(4.2-36)], or equivalently by the product of the first row of G1 by the firstcolumn of T as given by (4.2-37) for the three-dimensional case Because onlythe x, y coordinates of g1are nonzero, only the first two coordinates of t1enterinto this product, or equivalent, projection Thus we can think of g1 as a twoelement vector to get

Applying (11.1-6a) and (11.1-6b) to t0 of (11.1-11) yields

t0¼ ikt0k cos 1þ jkt0k sin 1 ð11:1-20ÞSubstituting (11.1-18) and (11.1-20) into (11.1-19) and using (11.1-9) yield

g1 t1¼ g1 t0 ¼ kt0k ¼ ðt11Þ1 ð11:1-21ÞThat is, the projection of t0into g1is equal to the magnitude of t0, as we saw itshould be from the above discussion relative to thinking of G1 as doing arotation of t0 This result also follows because, as indicated above, the unitvector g1 was chosen to lie not only in the x, y plane but also along the vector

t0; see (11.1-18) and (11.1-20)

Equation (11.1-21) gives us the first coordinate of the projection of t1 ontothe s-dimensional row space of G1 Now let us determine the second coordinateobtained by projecting t1 onto the second-row unit vector of G1, which wedesignate as g2 The vector g2 is also in the x, y plane of Figure 11.1-2a butperpendicular to g1 Specifically, from Figure 11.1-2a and (11.1-6) it followsthat

g2¼ i sin 1þ j cos 1þ k 0 þ 0 þ ð11:1-22ÞThe projection of t1 onto g2 is given by

and substituting (11.1-20) and (11.1-22) into the above equation yields

g2t1 ¼ sin 1 cos 1þ cos 1 sin 1 ¼ 0 ð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 g2is orthogonal to the direction of g1, which is lined upwith the direction of t1 and in turn t0

Equation (11.1-24) gives us the second coordinate of the projection of t1ontothe s-dimensional space of G1 Now we determine the third coordinate byprojecting t1 onto the third-row unit vector of G1, which we designate as g3.Examining (11.1-6), we see that the third row consists of zero entries except forthe third-column entry, which is a 1 Consequently, the projection of t1onto thethird-row unit vector g3 leaves the third coordinate of t1 unchanged Statedanother way, g3 is a unit vector aligned along the third coordinate of t1 Thisfollows because all the coordinates of g3 are zero except the third, which isunity Consequently, projecting t1 onto g3 leaves the third coordinate of t1unchanged The same is true for the remaining coordinates of t1; they areunchanged by the transformation G because the ith-row vector g, i > 2, is a

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 G1 is an s s matrix In this case, thelower right-hand corner ðs  2Þ  ðs  2Þ matrix of G1 is the identity matrixwith the elements to its left being zero Thus Figure 11.1-2a illustrates theprojection of the vector t0, defined by the first two coordinates of t1, onto thespace defined by the first two unit row vectors g1 and g2 of G1.

This completes the proof that G1 projects T into the form given by (11.1-2).Designate this projected vector t1 as ðt1Þ1 In summary, G1 transformsthe columns of T to a new coordinate system for which the coordinates of

T have the form given by (11.1-2) with ðt21Þ1 ¼ 0 We will let ðt1Þ1represent the column vector t1, the first column of T, in this new coordinatesystem

Let us carry one step further our discussion of viewing the simple GiGivens transformations as projecting the column space of the matrix T ontothe space defined by the row vectors of Gi This is now done by detailingthis projection for G2 On examining (11.1-13) it is seen that only the firstand third coordinates are altered by this projection, the unit vectors of G2represented by the other rows of G2 being unit vectors aligned along theother coordinates of the matrix T Now consider the unit vectors ðg1Þ2 and

ðg3Þ2 defined by the first and third rows of G2 On examining (11.1-16),(11.1-13a), (11.1-13b), and (11.1-13), it can be verified that ðg1Þ2 ischosen to line up with the vector defined by the first- and third-row coordi-nates of ðt1Þ1, with all other coordinates set equal to zero, while ðg3Þ2 ischosen to be orthogonal to ðg1Þ2 This situation is illustrated in Figure11.1-2b; see also Figure 11.1-2c In this figure t2 is the vector formed bythe first- and third-row coordinates of ðt1Þ1; see (11.1-2) and (11.1-16) As

a consequence of this choice forðg1Þ2, the projection of t2 ontoðg1Þ2, which

is the first coordinate of the vector G2ðt1Þ1, has a magnitude equal to themagnitude of t2, which is the desired ðt11Þ2 of (11.1-3); see also (11.1-14)

In turn t2 equals the magnitude of the first three coordinates of t1; see(11.1-14) Moreover, this last result follows because ðg1Þ2 in theabove is actually chosen to line up with the first three coordinates of thevector t1

Because ðg3Þ2 is orthogonal to ðg1Þ2 and in turn t2, the third coordinate

of G2ðt1Þ1 given by ðg3Þ2ðt1Þ1 ¼ ðg3Þ2t2, equals zero This results in thefirst column of G2G1T being in the form given in (11.1-3), as desired.Figure 11.1-2b illustrates the projection of the vector t2, defined by the vectors

t0 and kt31, onto the space defined by the first and third rows of G2, orequivalently, the unit vector rows ðg1Þ2 and ðg3Þ2 of G2 The physicalsignificance of the succeeding Givens transformations follows in a similarmanner This completes our interpretation of the Givens transformations as anorthogonal coordinate transformation In Chapter 13 we shall relate the Givenstransformation to the Gram–Schmidt transformation introduced in Section 4.3

In Chapter 12 we shall relate the Givens transformation to the Householdertransformation

The transformations can be conveniently applied to T and YðnÞ taneously by generating the augmented matrix T0 given by

and then applying the Givens orthonormal transformation to it [This parallelsthe procedure followed in Section 4.3 where such an augmented matrix wasgenerated to apply the Gram–Schmidt transformation to T and YðnÞ simul-taneously; see (4.3-54) and the discussions related to it] To be concrete, assumethat T is a 2 3 matrix given by

264

37

As shall be shown shortly, the square of the lower right element ðy3Þ3 is thenumerical value of the least-squares residue error given by (4.1-31), that is,

[This result was already shown in Section 4.3; specifically see (4.3-38).] The

2 2 upper left-hand corner matrix of t’s corresponds to the upper triangular

matrix of u’s of (10.2-8) or (10.2-9) We have used the entriesðtijÞ3 instead of

uijin matrix T00 given by (11.1-30) in order to emphasize that three elementaryGivens rotations are being used to transform the T matrix to the U matrix Thefollowing gives step by step the Givens orthonormal rotation transformationsfor a 3 2 matrix T : Let

35

(a) The first Givens rotation by matrix G1 is given as

37

37

37

where

c1¼ ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffit11

t2

11þ t2 21

p s1 ¼ ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffit21

t2

11þ t2 21p

ðt11Þ1¼

ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi

t2

11þ t2 21q

(b) The second Givens rotation by matrix G2 yields

37

37

ðt11Þ2 ¼

ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi

ðt11Þ21þ ðt31Þ2q

37

377775

37

an arbitrary constant This is what we are doing when using the Givens formation G1 of (11.1-6), with c1 and s1 given by (11.1-6a) and (11.1-6b) Inthis case the constant k is c1 This value of k was chosen over unity to makethe rows of G1 be unit vectors, which makes F an orthonormal transformation.Why do we want to make F an orthonormal matrix? It turns out that thesensitivity to computer round-off errors of our least-squares solution to(10.2-16) is minimized if the Gauss elimination form is obtained by the use of

trans-an orthonormal trtrans-ansformation, that is, if the matrices U trans-and Y10 are obtainedfrom T and YðnÞ using an orthonormal transformation matrix F [129]

3

We chose this T because it is one for which round-off errors can lead to aproblem when " is very small Specifically, when " is small enough, thecomputer round-off errors will cause the columns of T to be considered

essentially identical so as to lead to a degenerate situation of the type discussed

in Chapter 10 This is precisely the type of situation for which the Givensorthonormal transformation procedure is designed to work The T examplegiven by (11.2-1) was originally suggested by Golub [102; see also 79, p 91]

We shall pick a fairly large " for our example so as to be able to calculate theorthonormalized T on a hand calculator (such as the HP15C or HP32S) withoutrunning into computational round-off problems Toward this end we pick

"¼ 0:1 Applying three Givens rotations yields, for the transformed T matrix,

37

3

5 ¼ tt1121 tt1222

t31 t32

24

35

(a) For the first Givens rotation

3

5 ¼ 1:3453624050 1:3379294630:141225888

24

35

(b) For the second Givens rotation

3

5 ¼ 1:6763054610 1:6703399610:141225888

24

35

(c) For the third Givens rotation

3

5 ¼ 1:6763054610 1:6703399610:141295480

24

35

which agrees with (11.2-2)

It would be desirable to obtain an exact solution in terms of " for the matrix FT

n;n obtained using this T and the measured data Ingeneral, one would not have an exact solution However, for the simple examplegiven it is possible to get such an exact solution This is convenient because itallows examination of the relative accuracies of various algorithms beingdescribed in these sections for obtaining the least-squares solution when T isgiven by (11.2-1) The effect of different computer precision could also beexamined This is exactly what is done in reference 79 (pp 91–100) We willnext provide the exact solution to FT

It is possible to obtain the sought-after exact solution in terms of " usingthree Givens rotations of (11.2-1) After much straightforward algebra oneobtains [79, pp 91–96]

and

&¼ 6  4" þ "2 ð11:2-10Þ

Using the above results for (11.1-30) it is a straightforward matter to nowsolve (10.2-16) by the back-substitution method From the above (10.2-16)becomes

ðt11Þ3 ðt12Þ3

0 ðt22Þ3

1 2

CiTCj¼ UT

By evaluating (11.2-14) for all i, j inner products, it is a straightforward matter

to solve for all the terms of (11.1-30), giving the solutions of (11.2-3) through(11.2-10) Specifically one proceeds by first computing the inner products for

i¼ j ¼ 1 to solve for ðt11Þ3 Next the inner products for i¼ 1 and j ¼ 2,3 areobtained to solve for respectively ðt12Þ3 andðy1Þ3 Then we obtain the innerproduct for i¼ j ¼ 2 to solve for ðt22Þ3 The inner product for i=2 and j=3 thengivesðy2Þ3 Finally the last inner product of i¼ j ¼ 3 yields the ðy3Þ3 Thisprocedure is very much similar to the back-substitution procedure.]

11.3.1 Systolic Array

The Givens orthonormal transformation for triangularizing the augmentedmatrix T0 described above can be implemented using a parallel-circuitarchitecture called a systolic array that shall be defined shortly It should becautioned that the systolic array described here would not be used in those caseswhere the T matrix is known in advance; see the discussion at the end of Section