00001
00002
00003
00006
00007
00008
00009
00010 #include "include.h"
00011 #include "newmatap.h"
00012
00013 #include "tmt.h"
00014
00015 #ifdef use_namespace
00016 using namespace NEWMAT;
00017 #endif
00018
00019 ReturnMatrix Inverter1(const CroutMatrix& X)
00020 {
00021 Matrix Y = X.i();
00022 Y.Release();
00023 return Y.ForReturn();
00024 }
00025
00026
00027 ReturnMatrix Inverter2(CroutMatrix X)
00028 {
00029 Matrix Y = X.i();
00030 Y.Release();
00031 return Y.ForReturn();
00032 }
00033
00034 ReturnMatrix Inverter1(const BandLUMatrix& X)
00035 {
00036 Matrix Y = X.i();
00037 Y.Release();
00038 return Y.ForReturn();
00039 }
00040
00041
00042 ReturnMatrix Inverter2(BandLUMatrix X)
00043 {
00044 Matrix Y = X.i();
00045 Y.Release();
00046 return Y.ForReturn();
00047 }
00048
00049 ReturnMatrix LU1(const Matrix& A)
00050 {
00051 Tracer et1("LU1 - Crout");
00052 CroutMatrix X = A;
00053 return X.for_return();
00054 }
00055
00056 ReturnMatrix LU2(const Matrix& A)
00057 {
00058 Tracer et1("LU2 - Crout");
00059 CroutMatrix X = A; X.release();
00060 return X.for_return();
00061 }
00062
00063 ReturnMatrix LU3(const Matrix& A)
00064 {
00065 Tracer et1("LU3 - Crout");
00066 CroutMatrix* X = new CroutMatrix(A); MatrixErrorNoSpace(X);
00067 X->release_and_delete();
00068 return X->for_return();
00069 }
00070
00071 ReturnMatrix LU1(const BandMatrix& A)
00072 {
00073 Tracer et1("LU1 - BandLU");
00074 BandLUMatrix X = A;
00075 return X.for_return();
00076 }
00077
00078 ReturnMatrix LU2(const BandMatrix& A)
00079 {
00080 Tracer et1("LU2 - BandLU");
00081 BandLUMatrix X = A; X.release();
00082 return X.for_return();
00083 }
00084
00085 ReturnMatrix LU3(const BandMatrix& A)
00086 {
00087 Tracer et1("LU3 - BandLU");
00088 BandLUMatrix* X = new BandLUMatrix(A); MatrixErrorNoSpace(X);
00089 X->release_and_delete();
00090 return X->for_return();
00091 }
00092
00093
00094
00095 void CircularShift(const Matrix& X1, int first, int last)
00096 {
00097 Matrix X; UpperTriangularMatrix U1, U2;
00098 int n = X1.Ncols();
00099
00100
00101 X = X1; QRZ(X, U1);
00102 RightCircularUpdateCholesky(U1, first, last);
00103 X = X1.Columns(1,first-1) | X1.Column(last)
00104 | X1.Columns(first,last-1) | X1.Columns(last+1,n);
00105 QRZ(X, U2);
00106 X = U1 - U2; Clean(X, 0.000000001); Print(X);
00107
00108
00109 X = X1; QRZ(X, U1);
00110 LeftCircularUpdateCholesky(U1, first, last);
00111 X = X1.Columns(1,first-1) | X1.Columns(first+1,last)
00112 | X1.Column(first) | X1.Columns(last+1,n);
00113 QRZ(X, U2);
00114 X = U1 - U2; Clean(X, 0.000000001); Print(X);
00115 }
00116
00117 class TestUpdateQRZ
00118 {
00119 int m,n1,n2,n3;
00120 Matrix X1, X2, X3;
00121 MultWithCarry mwc;
00122 public:
00123 void Reset();
00124 TestUpdateQRZ(int mx, int n1x, int n2x=0, int n3x=0)
00125 : m(mx), n1(n1x), n2(n2x), n3(n3x) { Reset(); }
00126 void DoTest();
00127 void ClearRow(int i) { X1.Row(i) = 0.0; }
00128 void SetRow(int i, int j) { X1.Row(i) = X1.Row(j); }
00129 };
00130
00131
00132
00133 void trymatd()
00134 {
00135 Tracer et("Thirteenth test of Matrix package");
00136 Tracer::PrintTrace();
00137 Matrix X(5,20);
00138 int i,j;
00139 for (j=1;j<=20;j++) X(1,j) = j+1;
00140 for (i=2;i<=5;i++) for (j=1;j<=20; j++) X(i,j) = (long)X(i-1,j) * j % 1001;
00141 SymmetricMatrix S; S << X * X.t();
00142 Matrix SM = X * X.t() - S;
00143 Print(SM);
00144 LowerTriangularMatrix L = Cholesky(S);
00145 Matrix Diff = L*L.t()-S; Clean(Diff, 0.000000001);
00146 Print(Diff);
00147 {
00148 Tracer et1("Stage 1");
00149 LowerTriangularMatrix L1(5);
00150 Matrix Xt = X.t(); Matrix Xt2 = Xt;
00151 QRZT(X,L1);
00152 Diff = L - L1; Clean(Diff,0.000000001); Print(Diff);
00153 UpperTriangularMatrix Ut(5);
00154 QRZ(Xt,Ut);
00155 Diff = L - Ut.t(); Clean(Diff,0.000000001); Print(Diff);
00156 Matrix Y(3,20);
00157 for (j=1;j<=20;j++) Y(1,j) = 22-j;
00158 for (i=2;i<=3;i++) for (j=1;j<=20; j++)
00159 Y(i,j) = (long)Y(i-1,j) * j % 101;
00160 Matrix Yt = Y.t(); Matrix M,Mt; Matrix Y2=Y;
00161 QRZT(X,Y,M); QRZ(Xt,Yt,Mt);
00162 Diff = Xt - X.t(); Clean(Diff,0.000000001); Print(Diff);
00163 Diff = Yt - Y.t(); Clean(Diff,0.000000001); Print(Diff);
00164 Diff = Mt - M.t(); Clean(Diff,0.000000001); Print(Diff);
00165 Diff = Y2 * Xt2 * S.i() - M * L.i();
00166 Clean(Diff,0.000000001); Print(Diff);
00167 }
00168
00169 ColumnVector C1(5);
00170 {
00171 Tracer et1("Stage 2");
00172 X.ReSize(5,5);
00173 for (j=1;j<=5;j++) X(1,j) = j+1;
00174 for (i=2;i<=5;i++) for (j=1;j<=5; j++)
00175 X(i,j) = (long)X(i-1,j) * j % 1001;
00176 for (i=1;i<=5;i++) C1(i) = i*i;
00177 CroutMatrix A = X;
00178 ColumnVector C2 = A.i() * C1; C1 = X.i() * C1;
00179 X = C1 - C2; Clean(X,0.000000001); Print(X);
00180 }
00181
00182 {
00183 Tracer et1("Stage 3");
00184 X.ReSize(7,7);
00185 for (j=1;j<=7;j++) X(1,j) = j+1;
00186 for (i=2;i<=7;i++) for (j=1;j<=7; j++)
00187 X(i,j) = (long)X(i-1,j) * j % 1001;
00188 C1.ReSize(7);
00189 for (i=1;i<=7;i++) C1(i) = i*i;
00190 RowVector R1 = C1.t();
00191 Diff = R1 * X.i() - ( X.t().i() * R1.t() ).t(); Clean(Diff,0.000000001);
00192 Print(Diff);
00193 }
00194
00195 {
00196 Tracer et1("Stage 4");
00197 X.ReSize(5,5);
00198 for (j=1;j<=5;j++) X(1,j) = j+1;
00199 for (i=2;i<=5;i++) for (j=1;j<=5; j++)
00200 X(i,j) = (long)X(i-1,j) * j % 1001;
00201 C1.ReSize(5);
00202 for (i=1;i<=5;i++) C1(i) = i*i;
00203 CroutMatrix A1 = X*X;
00204 ColumnVector C2 = A1.i() * C1; C1 = X.i() * C1; C1 = X.i() * C1;
00205 X = C1 - C2; Clean(X,0.000000001); Print(X);
00206 }
00207
00208
00209 {
00210 Tracer et1("Stage 5");
00211 int n = 40;
00212 SymmetricBandMatrix B(n,2); B = 0.0;
00213 for (i=1; i<=n; i++)
00214 {
00215 B(i,i) = 6;
00216 if (i<=n-1) B(i,i+1) = -4;
00217 if (i<=n-2) B(i,i+2) = 1;
00218 }
00219 B(1,1) = 5; B(n,n) = 5;
00220 SymmetricMatrix A = B;
00221 ColumnVector X(n);
00222 X(1) = 429;
00223 for (i=2;i<=n;i++) X(i) = (long)X(i-1) * 31 % 1001;
00224 X = X / 100000L;
00225
00226
00227
00228 ColumnVector Y1 = A.i() * X;
00229 LowerTriangularMatrix C1 = Cholesky(A);
00230 ColumnVector Y2 = C1.t().i() * (C1.i() * X) - Y1;
00231 Clean(Y2, 0.000000001); Print(Y2);
00232 UpperTriangularMatrix CU = C1.t().i();
00233 LowerTriangularMatrix CL = C1.i();
00234 Y2 = CU * (CL * X) - Y1;
00235 Clean(Y2, 0.000000001); Print(Y2);
00236 Y2 = B.i() * X - Y1; Clean(Y2, 0.000000001); Print(Y2);
00237
00238 LowerBandMatrix C2 = Cholesky(B);
00239 Matrix M = C2 - C1; Clean(M, 0.000000001); Print(M);
00240 ColumnVector Y3 = C2.t().i() * (C2.i() * X) - Y1;
00241 Clean(Y3, 0.000000001); Print(Y3);
00242 CU = C1.t().i();
00243 CL = C1.i();
00244 Y3 = CU * (CL * X) - Y1;
00245 Clean(Y3, 0.000000001); Print(Y3);
00246
00247 Y3 = B.i() * X - Y1; Clean(Y3, 0.000000001); Print(Y3);
00248
00249 SymmetricMatrix AI = A.i();
00250 Y2 = AI*X - Y1; Clean(Y2, 0.000000001); Print(Y2);
00251 SymmetricMatrix BI = B.i();
00252 BandMatrix C = B; Matrix CI = C.i();
00253 M = A.i() - CI; Clean(M, 0.000000001); Print(M);
00254 M = B.i() - CI; Clean(M, 0.000000001); Print(M);
00255 M = AI-BI; Clean(M, 0.000000001); Print(M);
00256 M = AI-CI; Clean(M, 0.000000001); Print(M);
00257
00258 M = A; AI << M; M = AI-A; Clean(M, 0.000000001); Print(M);
00259 C = B; BI << C; M = BI-B; Clean(M, 0.000000001); Print(M);
00260 }
00261
00262 {
00263 Tracer et1("Stage 5");
00264 SymmetricMatrix A(4), B(4);
00265 A << 5
00266 << 1 << 4
00267 << 2 << 1 << 6
00268 << 1 << 0 << 1 << 7;
00269 B << 8
00270 << 1 << 5
00271 << 1 << 0 << 9
00272 << 2 << 1 << 0 << 6;
00273 LowerTriangularMatrix AB = Cholesky(A) * Cholesky(B);
00274 Matrix M = Cholesky(A) * B * Cholesky(A).t() - AB*AB.t();
00275 Clean(M, 0.000000001); Print(M);
00276 M = A * Cholesky(B); M = M * M.t() - A * B * A;
00277 Clean(M, 0.000000001); Print(M);
00278 }
00279
00280 {
00281 Tracer et1("Stage 6");
00282 int N=49;
00283 int i;
00284 SymmetricBandMatrix S(N,1);
00285 Matrix B(N,N+1); B=0;
00286 for (i=1;i<=N;i++) { S(i,i)=1; B(i,i)=1; B(i,i+1)=-1; }
00287 for (i=1;i<N; i++) S(i,i+1)=-.5;
00288 DiagonalMatrix D(N+1); D = 1;
00289 B = B.t()*S.i()*B - (D-1.0/(N+1))*2.0;
00290 Clean(B, 0.000000001); Print(B);
00291 }
00292
00293 {
00294 Tracer et1("Stage 7");
00295
00296 Matrix A(7,7);
00297 A.Row(1) << 3 << 2 << -1 << 4 << -3 << 5 << 9;
00298 A.Row(2) << -8 << 7 << 2 << 0 << 7 << 0 << -1;
00299 A.Row(3) << 2 << -2 << 3 << 1 << 9 << 0 << 3;
00300 A.Row(4) << -1 << 5 << 2 << 2 << 5 << -1 << 2;
00301 A.Row(5) << 4 << -4 << 1 << 9 << -8 << 7 << 5;
00302 A.Row(6) << 1 << -2 << 5 << -1 << -2 << 5 << 1;
00303 A.Row(7) << -6 << 3 << -1 << 8 << -1 << 2 << 2;
00304 RowVector D(30); D = 0;
00305 Real x = determinant(A);
00306 CroutMatrix B = A;
00307 D(1) = determinant(B) / x - 1;
00308 Matrix C = A * Inverter1(B) - IdentityMatrix(7);
00309 Clean(C, 0.000000001); Print(C);
00310
00311 CroutMatrix B1; B1 = B;
00312 D(2) = determinant(B1) / x - 1;
00313 C = A * Inverter2(B1) - IdentityMatrix(7);
00314 Clean(C, 0.000000001); Print(C);
00315
00316 B.release(); B1 = B;
00317 D(2) = B.nrows(); D(3) = B.ncols(); D(4) = B.size();
00318 D(5) = determinant(B1) / x - 1;
00319 B1.release();
00320 C = A * Inverter2(B1) - IdentityMatrix(7);
00321 D(6) = B1.nrows(); D(7) = B1.ncols(); D(8) = B1.size();
00322 Clean(C, 0.000000001); Print(C);
00323
00324 B1 = -A;
00325 D(9) = determinant(B1) / x + 1;
00326 C = -A * Inverter2(B1) - IdentityMatrix(7);
00327 Clean(C, 0.000000001); Print(C);
00328
00329 B = LU1(A); B1 = LU2(A); CroutMatrix B2 = LU3(A);
00330 C = A * B.i() - IdentityMatrix(7); Clean(C, 0.000000001); Print(C);
00331 D(10) = (B == B1 ? 0 : 1) + (B == B2 ? 0 : 1);
00332
00333 D(13) = B.size()-49;
00334
00335 B1.release(2);
00336 B2 = B1; D(15) = B == B2 ? 0 : 1;
00337 CroutMatrix B3 = B1; D(16) = B == B3 ? 0 : 1;
00338 D(17) = B1.size();
00339
00340 B1 = B; B1 = B1.i(); C = A - B1.i(); Clean(C, 0.000000001); Print(C);
00341 B1 = B; B1.release(); B1 = B1; B2 = B1;
00342 D(19) = B == B1 ? 0 : 1; D(20) = B == B2 ? 0 : 1;
00343 B1.cleanup(); B2 = B1; D(21) = B1.size(); D(22) = B2.size();
00344 GenericMatrix GM = B; C = A.i() - GM.i(); Clean(C, 0.000000001); Print(C);
00345 B1 = GM; D(23) = B == B1 ? 0 : 1;
00346 B1 = A * 0; B2 = B1; D(24) = B2.is_singular() ? 0 : 1;
00347
00348 const Real* d = B.const_data();
00349 const int* i = B.const_data_indx();
00350 B1 = B;
00351 const Real* d1 = B1.const_data();
00352 const int* i1 = B1.const_data_indx();
00353 B1.release(); B2 = B1;
00354 const Real* d2 = B2.const_data();
00355 const int* i2 = B2.const_data_indx();
00356 D(25) = (d != d1 ? 0 : 1) + (d1 == d2 ? 0 : 1)
00357 + (i != i1 ? 0 : 1) + (i1 == i2 ? 0 : 1);
00358
00359 Clean(D, 0.000000001); Print(D);
00360 }
00361
00362 {
00363 Tracer et1("Stage 8");
00364
00365 BandMatrix A(7,3,2);
00366 A.Row(1) << 3 << 2 << -1;
00367 A.Row(2) << -8 << 7 << 2 << 0;
00368 A.Row(3) << 2 << -2 << 3 << 1 << 9;
00369 A.Row(4) << -1 << 5 << 2 << 2 << 5 << -1;
00370 A.Row(5) << -4 << 1 << 9 << -8 << 7 << 5;
00371 A.Row(6) << 5 << -1 << -2 << 5 << 1;
00372 A.Row(7) << 8 << -1 << 2 << 2;
00373 RowVector D(30); D = 0;
00374 Real x = determinant(A);
00375 BandLUMatrix B = A;
00376 D(1) = determinant(B) / x - 1;
00377 Matrix C = A * Inverter1(B) - IdentityMatrix(7);
00378 Clean(C, 0.000000001); Print(C);
00379
00380 BandLUMatrix B1; B1 = B;
00381 D(2) = determinant(B1) / x - 1;
00382 C = A * Inverter2(B1) - IdentityMatrix(7);
00383 Clean(C, 0.000000001); Print(C);
00384
00385 B.release(); B1 = B;
00386 D(2) = B.nrows(); D(3) = B.ncols(); D(4) = B.size();
00387 D(5) = determinant(B1) / x - 1;
00388 B1.release();
00389 C = A * Inverter2(B1) - IdentityMatrix(7);
00390 D(6) = B1.nrows(); D(7) = B1.ncols(); D(8) = B1.size();
00391 Clean(C, 0.000000001); Print(C);
00392
00393 B1 = -A;
00394 D(9) = determinant(B1) / x + 1;
00395 C = -A * Inverter2(B1) - IdentityMatrix(7);
00396 Clean(C, 0.000000001); Print(C);
00397
00398 B = LU1(A); B1 = LU2(A); BandLUMatrix B2 = LU3(A);
00399 C = A * B.i() - IdentityMatrix(7); Clean(C, 0.000000001); Print(C);
00400 D(10) = (B == B1 ? 0 : 1) + (B == B2 ? 0 : 1);
00401
00402 D(11) = B.bandwidth().lower()-3;
00403 D(12) = B.bandwidth().upper()-2;
00404 D(13) = B.size()-42;
00405 D(14) = B.size2()-21;
00406
00407 B1.release(2);
00408 B2 = B1; D(15) = B == B2 ? 0 : 1;
00409 BandLUMatrix B3 = B1; D(16) = B == B3 ? 0 : 1;
00410 D(17) = B1.size();
00411
00412 CroutMatrix CM = A;
00413 C = CM.i() - B.i(); Clean(C, 0.000000001); Print(C);
00414 D(18) = determinant(CM) / x - 1;
00415
00416 B1 = B; CM = B1.i(); C = A - CM.i(); Clean(C, 0.000000001); Print(C);
00417 B1 = B; B1.release(); B1 = B1; B2 = B1;
00418 D(19) = B == B1 ? 0 : 1; D(20) = B == B2 ? 0 : 1;
00419 B1.cleanup(); B2 = B1; D(21) = B1.size(); D(22) = B2.size();
00420 GenericMatrix GM = B; C = A.i() - GM.i(); Clean(C, 0.000000001); Print(C);
00421 B1 = GM; D(23) = B == B1 ? 0 : 1;
00422 B1 = A * 0; B2 = B1; D(24) = B2.is_singular() ? 0 : 1;
00423
00424 const Real* d = B.const_data(); const Real* dd = B.const_data();
00425 const int* i = B.const_data_indx();
00426 B1 = B;
00427 const Real* d1 = B1.const_data(); const Real* dd1 = B1.const_data();
00428 const int* i1 = B1.const_data_indx();
00429 B1.release(); B2 = B1;
00430 const Real* d2 = B2.const_data(); const Real* dd2 = B2.const_data();
00431 const int* i2 = B2.const_data_indx();
00432 D(25) = (d != d1 ? 0 : 1) + (d1 == d2 ? 0 : 1)
00433 + (dd != dd1 ? 0 : 1) + (dd1 == dd2 ? 0 : 1)
00434 + (i != i1 ? 0 : 1) + (i1 == i2 ? 0 : 1);
00435
00436 Clean(D, 0.000000001); Print(D);
00437 }
00438
00439 {
00440 Tracer et1("Stage 9");
00441
00442
00443 int i, j;
00444
00445
00446 Matrix X(100, 10);
00447 MultWithCarry mwc;
00448 for (i = 1; i <= 100; ++i) for (j = 1; j <= 10; ++j)
00449 X(i, j) = 2.0 * (mwc.Next() - 0.5);
00450 Matrix X1 = X;
00451
00452
00453 SymmetricMatrix A; A << X.t() * X;
00454 UpperTriangularMatrix U1 = Cholesky(A).t();
00455
00456
00457 UpperTriangularMatrix U2;
00458 QRZ(X, U2);
00459 Matrix Diff = U1 - U2; Clean(Diff, 0.000000001); Print(Diff);
00460
00461
00462 RowVector NewRow(10);
00463 for (j = 1; j <= 10; ++j) NewRow(j) = 2.0 * (mwc.Next() - 0.5);
00464 UpdateCholesky(U2, NewRow);
00465 X = X1 & NewRow; QRZ(X, U1);
00466 Diff = U1 - U2; Clean(Diff, 0.000000001); Print(Diff);
00467
00468
00469 DowndateCholesky(U2, X1.Row(20));
00470 DowndateCholesky(U2, X1.Row(35));
00471 X = X1.Rows(1,19) & X1.Rows(21,34) & X1.Rows(36,100) & NewRow; QRZ(X, U1);
00472 Diff = U1 - U2; Clean(Diff, 0.000000001); Print(Diff);
00473
00474
00475
00476 CircularShift(X, 3,6);
00477 CircularShift(X, 5,5);
00478 CircularShift(X, 4,5);
00479 CircularShift(X, 1,6);
00480 CircularShift(X, 6,10);
00481 }
00482
00483 {
00484 Tracer et1("Stage 10");
00485
00486 TestUpdateQRZ tuqrz1(10, 100, 50, 25); tuqrz1.DoTest();
00487 tuqrz1.Reset(); tuqrz1.ClearRow(1); tuqrz1.DoTest();
00488 tuqrz1.Reset(); tuqrz1.ClearRow(1); tuqrz1.ClearRow(2); tuqrz1.DoTest();
00489 tuqrz1.Reset(); tuqrz1.ClearRow(5); tuqrz1.ClearRow(6); tuqrz1.DoTest();
00490 tuqrz1.Reset(); tuqrz1.ClearRow(10); tuqrz1.DoTest();
00491 TestUpdateQRZ tuqrz2(15, 100, 0, 0); tuqrz2.DoTest();
00492 tuqrz2.Reset(); tuqrz2.ClearRow(1); tuqrz2.DoTest();
00493 tuqrz2.Reset(); tuqrz2.ClearRow(1); tuqrz2.ClearRow(2); tuqrz2.DoTest();
00494 tuqrz2.Reset(); tuqrz2.ClearRow(5); tuqrz2.ClearRow(6); tuqrz2.DoTest();
00495 tuqrz2.Reset(); tuqrz2.ClearRow(15); tuqrz2.DoTest();
00496 TestUpdateQRZ tuqrz3(5, 0, 10, 0); tuqrz3.DoTest();
00497
00498 }
00499
00500
00501 }
00502
00503 void TestUpdateQRZ::Reset()
00504 {
00505 int i, j;
00506
00507 X1.ReSize(m, n1); X2.ReSize(m, n2); X3.ReSize(m, n3);
00508 for (i = 1; i <= m; ++i) for (j = 1; j <= n1; ++j)
00509 X1(i, j) = 2.0 * (mwc.Next() - 0.5);
00510 for (i = 1; i <= m; ++i) for (j = 1; j <= n2; ++j)
00511 X2(i, j) = 2.0 * (mwc.Next() - 0.5);
00512 for (i = 1; i <= m; ++i) for (j = 1; j <= n3; ++j)
00513 X3(i, j) = 2.0 * (mwc.Next() - 0.5);
00514 }
00515
00516 void TestUpdateQRZ::DoTest()
00517 {
00518 Matrix XT1 = X1.t(), XT2 = X2.t(), XT3 = X3.t();
00519 Matrix X = X1 | X2 | X3;
00520 SymmetricMatrix SM; SM << X * X.t();
00521 LowerTriangularMatrix L, LX, LY, L0;
00522 QRZT(X, L);
00523 X = X1 | X2 | X3;
00524 LY.ReSize(m); LY = 0.0;
00525 UpdateQRZT(X, LY);
00526 QRZT(X1, LX); UpdateQRZT(X2, LX); UpdateQRZT(X3, LX);
00527 Matrix Diff = L * L.t() - SM; Clean(Diff, 0.000000001); Print(Diff);
00528 Diff = SM - LX * LX.t(); Clean(Diff, 0.000000001); Print(Diff);
00529 Diff = SM - LY * LY.t(); Clean(Diff, 0.000000001); Print(Diff);
00530 UpperTriangularMatrix U, UX, UY;
00531 X = XT1 & XT2 & XT3;
00532 QRZ(X, U);
00533 Diff = U.t() * U - SM; Clean(Diff, 0.000000001); Print(Diff);
00534 UY.ReSize(m); UY = 0.0;
00535 X = XT1 & XT2 & XT3;
00536 UpdateQRZ(X, UY);
00537 Diff = SM - UY.t() * UY; Clean(Diff, 0.000000001); Print(Diff);
00538 QRZ(XT1, UX); UpdateQRZ(XT2, UX); UpdateQRZ(XT3, UX);
00539 Diff = SM - UX.t() * UX; Clean(Diff, 0.000000001); Print(Diff);
00540 }
00541