linear_algebra.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2012 Swift Navigation Inc.
00003  * Contact: Matt Peddie <peddie@alum.mit.edu>
00004  *
00005  * This source is subject to the license found in the file 'LICENSE' which must
00006  * be be distributed together with this source. All other rights reserved.
00007  *
00008  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
00009  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
00010  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
00011  */
00012 
00013 #include <math.h>
00014 #include <string.h>
00015 #include <stdio.h>
00016 
00017 #include "common.h"
00018 
00019 #include "linear_algebra.h"
00020 
00021 #define VEC_PRINTF(v, _n) {                                     \
00022     printf("%s:%u <|%s| %lf",                                   \
00023            __FILE__, __LINE__, #v, v[0]);                       \
00024     for (u32 _i = 1; _i < _n; _i++) printf(", %lf", v[_i]);     \
00025     printf(">\n");                                              \
00026   }
00027 
00028 #define MAT_PRINTF(m, _r, _c) {                    \
00029     printf("%s:%u <|%s|",                          \
00030            __FILE__, __LINE__, #m);                \
00031     for (u32 _i = 0; _i < _r; _i++) {              \
00032       printf(" [%lf", m[_i*_c + 0]);                \
00033       for (u32 _j = 1; _j < _c; _j++)              \
00034         printf(" %lf", m[_i*_c + _j]);               \
00035       printf("]");                                 \
00036       if (_r > 2 && _i < _r - 1) printf("\n\t\t\t");              \
00037     }                                              \
00038     printf(">\n");                                 \
00039   }
00040 
00041 /* Todo(MP) -- Implement fast linear solve (all-in-one) with Cholesky
00042  * decomposition: we want to solve $A^{T} W A \hat{x} = A^{T} W y
00043  *
00044  * This should be implemented below the functions that explicitly
00045  * calculate the pseudoinverse: we have a function that computes the
00046  * Cholesky decomposition, a function that performs back-substitution
00047  * for the linear solve and then a whole slew of interface functions
00048  * for least-squares and least-norm solves.  Separate functions are
00049  * required for testing against; a separate "fast" weighted linear
00050  * least-squares operation should perform all operations in one
00051  * function (lazily or orderedly accessing elements from A and W to
00052  * form A W A^T as we go).
00053  */
00054 
00072 #define MATRIX_EPSILON (1e-11)
00073 
00074 /* \} */
00075 
00103 s32 qrdecomp(const double *a, u32 rows, u32 cols, double *qt, double *r) {
00104   s32 sing = 0;
00105   u32 i, j, k;
00106 
00107   double c[cols], d[cols];
00108   double scale, sigma, sum, tau;
00109   /* r := a */
00110   memcpy(r, a, rows*cols * sizeof(*r));
00111 
00112   for (k = 0; k < cols; k++) {
00113     if (k > cols-1) printf("%d: k (%u) > cols - 1 (%u)\n",
00114                            __LINE__, (unsigned int) k, (unsigned int) cols - 1);
00115     /* This is column k of the input matrix */
00116     scale = 0.0;
00117     /* loop over this column and make sure it's not all zeros*/
00118     for (i = k; i < rows; i++) scale = fmax(scale, fabs(r[i*cols + k]));
00119     /* singularity check */
00120     if (fabs(scale) < MATRIX_EPSILON) {
00121       sing = -1;
00122       c[k] = d[k] = 0.0;
00123       printf("Column %u is singular to machine precision.\n", (unsigned int) k);
00124     } else {
00125       /* scale this column to 1 */
00126       for (i = k; i < rows; i++) r[i*cols + k] /= scale;
00127       /* compute norm-squared of this column */
00128       for (sum = 0.0, i = k; i < rows; i++) sum += r[i*cols + k]*r[i*cols + k];
00129       /* adjust the leading element: σ is the sign of the leading
00130        * (diagonal) element of the column times the norm of the
00131        * head-adjusted column */
00132       sigma = copysign(sqrt(sum), r[k*cols + k]);
00133       r[k*cols + k] += sigma;
00134       /* set c and d for this column */
00135       c[k] = sigma * r[k*cols + k];
00136       d[k] = -scale * sigma;
00137       printf("d[%u] = %lf\n", (unsigned int) k, d[k]);
00138       for (j = k + 1; j < cols; j++) {
00139         if (j > cols-1) printf("%d: j (%u) > cols - 1 (%u)\n",
00140                                __LINE__, (unsigned int) j,
00141                                (unsigned int) cols - 1);
00142         /* some crazy shit to update R in place I think; we never
00143          * explicitly form the householder matrices */
00144         for (sum = 0.0, i = k; i < rows; i++) sum += r[i*cols + k]*r[i*cols + j];
00145         tau = sum / c[k];
00146         for (i = k; i < rows; i++) r[i*cols + j] -= tau*r[i*cols + k];
00147       }
00148     }
00149   }
00150   VEC_PRINTF(d, cols);
00151   VEC_PRINTF(c, cols);
00152   MAT_PRINTF(r, rows, cols);
00153   /* set last element of d to the bottom right corner of the matrix */
00154   /* d[cols - 1] = r[(rows - 1)*cols + cols - 1]; */
00155   printf("bottom corner: r[%u] = d[%u] = %lf\n",
00156          (unsigned int) ((rows - 1)*cols + cols - 1),
00157          (unsigned int) (cols - 1), r[(rows - 1)*cols + cols - 1]);
00158   /* if that's zero too, then the whole bottom row is zero and this
00159    * problem is singular */
00160   if (fabs(d[cols - 1]) < MATRIX_EPSILON) sing = -1;
00161   /* set Q^T to identity */
00162   for (i = 0; i < rows; i++) {
00163     for (j = 0; j < rows; j++) qt[i*rows + j] = 0.0;
00164     qt[i*rows + i] = 1.0;
00165   }
00166   for (k = 0; k < cols; k++) {
00167     if (k > cols-1) printf("%d: k (%u) > cols - 1 (%u)\n",
00168                            __LINE__, (unsigned int) k, (unsigned int) cols - 1);
00169     /* for column k of the matrix */
00170     if (fabs(c[k]) > MATRIX_EPSILON)
00171       /* if there's anything going on in this column */
00172       for (j = 0; j < rows; j++) {
00173         /* matrix multiply in place or something to form qt */
00174         sum = 0.0;
00175         for (i = k; i < rows; i++) sum += r[i*cols + k]*qt[i*rows + j];
00176         sum /= c[k];
00177         for (i = k; i < rows; i++) qt[i*rows + j] -= sum*r[i*cols + k];
00178       }
00179     MAT_PRINTF(qt, rows, rows);
00180   }
00181   for (i = 0; i < cols; i++) {
00182     if (i > cols-1) printf("%d: i (%u) > cols - 1 (%u)\n",
00183                            __LINE__, (unsigned int) i, (unsigned int) cols - 1);
00184     /* set r diagonal to d */
00185     r[i*cols + i] = d[i];
00186     printf("r[%u] = d[%u] = %lf\n", (unsigned int) (i*cols + i),
00187            (unsigned int) i, r[i*cols+i]);
00188   }
00189   for (i = 0; i < rows; i++)
00190     /* set below-diagonal elements to zero */
00191     for (j = 0; j < i && j < cols; j++) {
00192       if (j > cols-1) printf("%d: j (%u) > cols - 1 (%u)\n",
00193                              __LINE__, (unsigned int) j,
00194                              (unsigned int) cols - 1);
00195 
00196       r[i*cols + j] = 0.0;
00197     }
00198   return sing;
00199 }
00200 
00201 
00202 /* static s32 qrdecomp_l(const double *a, u32 rows, */
00203 /*                       u32 cols, double *qt, double *r) { */
00204 /*   s32 sing = 0; */
00205 /*   u32 i, j, k; */
00206 
00207 /*   double scale, anorm, vnorm; */
00208 /*   double u[rows], Hk[rows*rows]; */
00209 
00210 /*   /\* R := A *\/ */
00211 /*   memcpy(r, a, rows*cols*sizeof(*r)); */
00212 /*   /\* Q := I *\/ */
00213 /*   for (i = 0; i < rows; i++) */
00214 /*     for (j = 0; j < rows; j++) { */
00215 /*       qt[i*rows + j] = (i == j); */
00216 /*       Hk[i*rows + j] = 0; */
00217 /*     } */
00218 
00219 /*   for (k = 0; k < rows - 1; k++) { */
00220 /*     /\* Column k of the input matrix *\/ */
00221 /*     scale = 0.0; */
00222 /*     anorm = 0.0; */
00223 /*     /\* compute a^T a *\/ */
00224 /*     for (i = k; i < rows; i++) anorm += a[i*cols + k]*a[i*cols+k]; */
00225 /*     anorm = sqrt(anorm); */
00226 /*     /\* compute u = a + sgn(a[k]) * ||a||_2 * [1 0 0 . . . ]^T *\/ */
00227 /*     u[k] = a[k*cols + k] + copysign(a[k*cols + k], anorm); */
00228 /*     /\* form v = u / u[0] *\/ */
00229 /*     for (i = k+1; i < rows; i++) u[i] = a[i*cols + k] / u[k]; */
00230 /*     u[k] = 1.0; */
00231 /*     vnorm = 0.0; */
00232 /*     /\* compute w = v^T v *\/ */
00233 /*     for (i = k; i < rows; i++) vnorm += u[i]*u[i]; */
00234 /*     for (i = k; i < rows; i++) */
00235 /*       /\* yes, rows is the limit because H is square *\/ */
00236 /*       for (j = k; j < rows; j++) */
00237 /*         /\* Hk = I - 2 / w * v * v^T *\/ */
00238 /*         Hk[i*cols + j] = (i == j) - (2 / vnorm)*u[i]*u[j]; */
00239 /*     /\* Accumulate onto R *\/ */
00240 /*   } */
00241 /*   return sing; */
00242 /* } */
00243 
00257 s32 qrdecomp_square(const double *a, u32 rows, double *qt, double *r) {
00258   s32 sing = 0;
00259   u32 i, j, k;
00260 
00261   double c[rows], d[rows];
00262   double scale, sigma, sum, tau;
00263   memcpy(r, a, rows*rows * sizeof(*r));
00264 
00265   for (k = 0; k < rows - 1; k++) {
00266     scale = 0.0;
00267     for (i = k; i < rows; i++) scale = fmax(scale, fabs(r[i*rows + k]));
00268     if (scale == 0.0) {
00269       sing = -11;
00270       c[k] = d[k] = 0.0;
00271     } else {
00272       for (i = k; i < rows; i++) r[i*rows + k] /= scale;
00273       for (sum = 0.0, i = k; i < rows; i++) sum += r[i*rows + k]*r[i*rows + k];
00274       sigma = copysign(sqrt(sum), r[k*rows+k]);
00275       r[k*rows + k] += sigma;
00276       c[k] = sigma * r[k*rows + k];
00277       d[k] = -scale * sigma;
00278       for (j = k + 1; j < rows; j++) {
00279         for (sum = 0.0, i = k; i < rows; i++) sum += r[i*rows + k]*r[i*rows+j];
00280         tau = sum / c[k];
00281         for (i = k; i < rows; i++) r[i*rows + j] -= tau*r[i*rows + k];
00282       }
00283     }
00284   }
00285   d[rows - 1] = r[(rows - 1)*rows + rows - 1];
00286   if (d[rows - 1] == 0.0) sing = -11;
00287   for (i = 0; i < rows; i++) {
00288     for (j = 0; j < rows; j++) qt[i*rows + j] = 0.0;
00289     qt[i*rows + i] = 1.0;
00290   }
00291   for (k = 0; k < rows - 1; k++) {
00292     if (c[k] != 0.0)
00293       for (j = 0; j < rows; j++) {
00294         sum = 0.0;
00295         for (i = k; i < rows; i++) sum += r[i*rows + k]*qt[i*rows + j];
00296         sum /= c[k];
00297         for (i = k; i < rows; i++) qt[i*rows + j] -= sum*r[i*rows + k];
00298       }
00299   }
00300   for (i = 0; i < rows; i++) {
00301     r[i*rows + i] = d[i];
00302     for (j = 0; j < i; j++) r[i*rows + j] = 0.0;
00303   }
00304   return sing;
00305 }
00306 
00317 void qtmult(const double *qt, u32 n, const double *b, double *x) {
00318   u32 i, j;
00319   double sum;
00320   for (i = 0; i < n; i++) {
00321     sum = 0.0;
00322     for (j = 0; j < n; j++) sum += qt[i*n + j]*b[j];
00323     x[i] = sum;
00324   }
00325 }
00326 
00341 void rsolve(const double *r, u32 rows, u32 cols, const double *b, double *x) {
00342   s32 i, j;
00343   double sum;
00344   for (i = rows - 1; i >= 0; i--) {
00345     sum = b[i];
00346     for (j = i + 1; j < (int) rows; j++) sum -= r[i*cols + j]*x[j];
00347     x[i] = sum / r[i*cols + i];
00348   }
00349 }
00350 
00365 s32 qrsolve(const double *a, u32 rows, u32 cols, const double *b, double *x) {
00366   double qt[rows * rows], r[rows * cols];
00367   s32 sing = qrdecomp_square(a, rows, qt, r);
00368   if (sing != 0) return sing;
00369   qtmult(qt, rows, b, x);
00370   rsolve(r, rows, cols, x, x);
00371   return sing;
00372 }
00373 
00374 
00375 
00384 static inline int inv2(const double *a, double *b) {
00385   double det = a[0]*a[3] - a[1]*a[2];
00386   if (det < MATRIX_EPSILON)
00387     return -1;
00388   b[0] = a[3]/det;
00389   b[1] = -a[1]/det;
00390   b[2] = -a[2]/det;
00391   b[3] = a[0]/det;
00392   return 0;
00393 }
00394 
00403 static inline int inv3(const double *a, double *b) {
00404   double det = ((a[3*1 + 0]*-(a[3*0 + 1]*a[3*2 + 2]-a[3*0 + 2]*a[3*2 + 1])
00405                  +a[3*1 + 1]*(a[3*0 + 0]*a[3*2 + 2]-a[3*0 + 2]*a[3*2 + 0]))
00406                 +a[3*1 + 2]*-(a[3*0 + 0]*a[3*2 + 1]-a[3*0 + 1]*a[3*2 + 0]));
00407 
00408   if (det < MATRIX_EPSILON)
00409     return -1;
00410 
00411   b[3*0 + 0] = (a[3*1 + 1]*a[3*2 + 2]-a[3*1 + 2]*a[3*2 + 1])/det;
00412   b[3*1 + 0] = -(a[3*1 + 0]*a[3*2 + 2]-a[3*1 + 2]*a[3*2 + 0])/det;
00413   b[3*2 + 0] = (a[3*1 + 0]*a[3*2 + 1]-a[3*1 + 1]*a[3*2 + 0])/det;
00414 
00415   b[3*0 + 1] = -(a[3*0 + 1]*a[3*2 + 2]-a[3*0 + 2]*a[3*2 + 1])/det;
00416   b[3*1 + 1] = (a[3*0 + 0]*a[3*2 + 2]-a[3*0 + 2]*a[3*2 + 0])/det;
00417   b[3*2 + 1] = -(a[3*0 + 0]*a[3*2 + 1]-a[3*0 + 1]*a[3*2 + 0])/det;
00418 
00419   b[3*0 + 2] = (a[3*0 + 1]*a[3*1 + 2]-a[3*0 + 2]*a[3*1 + 1])/det;
00420   b[3*1 + 2] = -(a[3*0 + 0]*a[3*1 + 2]-a[3*0 + 2]*a[3*1 + 0])/det;
00421   b[3*2 + 2] = (a[3*0 + 0]*a[3*1 + 1]-a[3*0 + 1]*a[3*1 + 0])/det;
00422 
00423   return 0;
00424 }
00425 
00434 static inline int inv4(const double *a, double *b) {
00435   double det = (((a[4*1 + 0]*-((a[4*2 + 1]*-(a[4*0 + 2]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 2])+a[4*2 + 2]*(a[4*0 + 1]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 1]))+a[4*2 + 3]*-(a[4*0 + 1]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 1]))+a[4*1 + 1]*((a[4*2 + 0]*-(a[4*0 + 2]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 2])+a[4*2 + 2]*(a[4*0 + 0]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 0]))+a[4*2 + 3]*-(a[4*0 + 0]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 0])))+a[4*1 + 2]*-((a[4*2 + 0]*-(a[4*0 + 1]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 1])+a[4*2 + 1]*(a[4*0 + 0]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 0]))+a[4*2 + 3]*-(a[4*0 + 0]*a[4*3 + 1]-a[4*0 + 1]*a[4*3 + 0])))+a[4*1 + 3]*((a[4*2 + 0]*-(a[4*0 + 1]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 1])+a[4*2 + 1]*(a[4*0 + 0]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 0]))+a[4*2 + 2]*-(a[4*0 + 0]*a[4*3 + 1]-a[4*0 + 1]*a[4*3 + 0])));
00436 
00437   if (det < MATRIX_EPSILON)
00438     return -1;
00439 
00440   b[4*0 + 0] = ((a[4*2 + 1]*-(a[4*1 + 2]*a[4*3 + 3]-a[4*1 + 3]*a[4*3 + 2])+a[4*2 + 2]*(a[4*1 + 1]*a[4*3 + 3]-a[4*1 + 3]*a[4*3 + 1]))+a[4*2 + 3]*-(a[4*1 + 1]*a[4*3 + 2]-a[4*1 + 2]*a[4*3 + 1]))/det;
00441   b[4*1 + 0] = -((a[4*2 + 0]*-(a[4*1 + 2]*a[4*3 + 3]-a[4*1 + 3]*a[4*3 + 2])+a[4*2 + 2]*(a[4*1 + 0]*a[4*3 + 3]-a[4*1 + 3]*a[4*3 + 0]))+a[4*2 + 3]*-(a[4*1 + 0]*a[4*3 + 2]-a[4*1 + 2]*a[4*3 + 0]))/det;
00442   b[4*2 + 0] = ((a[4*2 + 0]*-(a[4*1 + 1]*a[4*3 + 3]-a[4*1 + 3]*a[4*3 + 1])+a[4*2 + 1]*(a[4*1 + 0]*a[4*3 + 3]-a[4*1 + 3]*a[4*3 + 0]))+a[4*2 + 3]*-(a[4*1 + 0]*a[4*3 + 1]-a[4*1 + 1]*a[4*3 + 0]))/det;
00443   b[4*3 + 0] = -((a[4*2 + 0]*-(a[4*1 + 1]*a[4*3 + 2]-a[4*1 + 2]*a[4*3 + 1])+a[4*2 + 1]*(a[4*1 + 0]*a[4*3 + 2]-a[4*1 + 2]*a[4*3 + 0]))+a[4*2 + 2]*-(a[4*1 + 0]*a[4*3 + 1]-a[4*1 + 1]*a[4*3 + 0]))/det;
00444 
00445   b[4*0 + 1] = -((a[4*2 + 1]*-(a[4*0 + 2]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 2])+a[4*2 + 2]*(a[4*0 + 1]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 1]))+a[4*2 + 3]*-(a[4*0 + 1]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 1]))/det;
00446   b[4*1 + 1] = ((a[4*2 + 0]*-(a[4*0 + 2]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 2])+a[4*2 + 2]*(a[4*0 + 0]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 0]))+a[4*2 + 3]*-(a[4*0 + 0]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 0]))/det;
00447   b[4*2 + 1] = -((a[4*2 + 0]*-(a[4*0 + 1]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 1])+a[4*2 + 1]*(a[4*0 + 0]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 0]))+a[4*2 + 3]*-(a[4*0 + 0]*a[4*3 + 1]-a[4*0 + 1]*a[4*3 + 0]))/det;
00448   b[4*3 + 1] = ((a[4*2 + 0]*-(a[4*0 + 1]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 1])+a[4*2 + 1]*(a[4*0 + 0]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 0]))+a[4*2 + 2]*-(a[4*0 + 0]*a[4*3 + 1]-a[4*0 + 1]*a[4*3 + 0]))/det;
00449 
00450   b[4*0 + 2] = ((a[4*1 + 1]*-(a[4*0 + 2]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 2])+a[4*1 + 2]*(a[4*0 + 1]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 1]))+a[4*1 + 3]*-(a[4*0 + 1]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 1]))/det;
00451   b[4*1 + 2] = -((a[4*1 + 0]*-(a[4*0 + 2]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 2])+a[4*1 + 2]*(a[4*0 + 0]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 0]))+a[4*1 + 3]*-(a[4*0 + 0]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 0]))/det;
00452   b[4*2 + 2] = ((a[4*1 + 0]*-(a[4*0 + 1]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 1])+a[4*1 + 1]*(a[4*0 + 0]*a[4*3 + 3]-a[4*0 + 3]*a[4*3 + 0]))+a[4*1 + 3]*-(a[4*0 + 0]*a[4*3 + 1]-a[4*0 + 1]*a[4*3 + 0]))/det;
00453   b[4*3 + 2] = -((a[4*1 + 0]*-(a[4*0 + 1]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 1])+a[4*1 + 1]*(a[4*0 + 0]*a[4*3 + 2]-a[4*0 + 2]*a[4*3 + 0]))+a[4*1 + 2]*-(a[4*0 + 0]*a[4*3 + 1]-a[4*0 + 1]*a[4*3 + 0]))/det;
00454 
00455   b[4*0 + 3] = -((a[4*1 + 1]*-(a[4*0 + 2]*a[4*2 + 3]-a[4*0 + 3]*a[4*2 + 2])+a[4*1 + 2]*(a[4*0 + 1]*a[4*2 + 3]-a[4*0 + 3]*a[4*2 + 1]))+a[4*1 + 3]*-(a[4*0 + 1]*a[4*2 + 2]-a[4*0 + 2]*a[4*2 + 1]))/det;
00456   b[4*1 + 3] = ((a[4*1 + 0]*-(a[4*0 + 2]*a[4*2 + 3]-a[4*0 + 3]*a[4*2 + 2])+a[4*1 + 2]*(a[4*0 + 0]*a[4*2 + 3]-a[4*0 + 3]*a[4*2 + 0]))+a[4*1 + 3]*-(a[4*0 + 0]*a[4*2 + 2]-a[4*0 + 2]*a[4*2 + 0]))/det;
00457   b[4*2 + 3] = -((a[4*1 + 0]*-(a[4*0 + 1]*a[4*2 + 3]-a[4*0 + 3]*a[4*2 + 1])+a[4*1 + 1]*(a[4*0 + 0]*a[4*2 + 3]-a[4*0 + 3]*a[4*2 + 0]))+a[4*1 + 3]*-(a[4*0 + 0]*a[4*2 + 1]-a[4*0 + 1]*a[4*2 + 0]))/det;
00458   b[4*3 + 3] = ((a[4*1 + 0]*-(a[4*0 + 1]*a[4*2 + 2]-a[4*0 + 2]*a[4*2 + 1])+a[4*1 + 1]*(a[4*0 + 0]*a[4*2 + 2]-a[4*0 + 2]*a[4*2 + 0]))+a[4*1 + 2]*-(a[4*0 + 0]*a[4*2 + 1]-a[4*0 + 1]*a[4*2 + 0]))/det;
00459 
00460   return 0;
00461 }
00462 
00463 /* Apparently there is no C code online that simply inverts a general
00464  * matrix without having to include GSL or something.  Therefore, I
00465  * give you Gaussian Elimination for matrix inversion.  --MP */
00466 
00467 /* Helper function for rref */
00468 static void row_swap(double *a, double *b, u32 size) {
00469   double tmp;
00470   for (u32 i = 0; i < size; i++) {
00471     tmp = a[i];
00472     a[i] = b[i];
00473     b[i] = tmp;
00474   }
00475 }
00476 
00477 /* rref is "reduced row echelon form" -- a helper function for the
00478  * gaussian elimination code. */
00479 static int rref(u32 order, u32 cols, double *m) {
00480   int i, j, k, maxrow;
00481   double tmp;
00482 
00483   for (i = 0; i < (int) order; i++) {
00484     maxrow = i;
00485     for (j = i+1; j < (int) order; j++) {
00486       /* Find the maximum pivot */
00487       if (fabs(m[j*cols+i]) > fabs(m[maxrow*cols+i]))
00488         maxrow = j;
00489     }
00490     row_swap(&m[i*cols], &m[maxrow*cols], cols);
00491     if (fabs(m[i*cols+i]) <= MATRIX_EPSILON) {
00492       /* If we've eliminated our diagonal element, it means our matrix
00493        * isn't full-rank.  Pork chop sandwiches!  */
00494       return -1;
00495     }
00496     for (j = i+1; j < (int) order; j++) {
00497       /* Elimination of column i */
00498       tmp = m[j*cols+i] / m[i*cols+i];
00499       for (k = i; k < (int) cols; k++) {
00500         m[j*cols+k] -= m[i*cols+k] * tmp;
00501       }
00502     }
00503   }
00504   for (i = order-1; i >= 0; i--) {
00505     /* Back-substitution */
00506     tmp = m[i*cols+i];
00507     for (j = 0; j < i; j++) {
00508       for (k = cols-1; k > i-1; k--) {
00509         m[j*cols+k] -= m[i*cols+k] * m[j*cols+i] / tmp;
00510       }
00511     }
00512     m[i*cols+i] /= tmp;
00513     for (j = order; j < (int) cols; j++) {
00514       /* Normalize row */
00515       m[i*cols+j] /= tmp;
00516     }
00517   }
00518   return 0;
00519 }
00520 
00534 inline int matrix_inverse(u32 n, const double const *a, double *b) {
00535   /* This function is currently only used to do a linear least-squares
00536    * solve for x, y, z and t in the navigation filter.  Gauss-Jordan
00537    * elimination is not the most efficient way to do this.  In the
00538    * ideal case, we'd use the Cholesky decomposition to compute the
00539    * least-squares fit.  (This may apply also to a least-norm fit if
00540    * we have too few satellites.)  The Cholesky decomposition becomes
00541    * even more important for unscented filters. */
00542   int res;
00543   u32 i, j, k, cols = n*2;
00544   double m[n*cols];
00545 
00546   /* For now, we special-case only small matrices.  If we bring back
00547    * multiple antennas, it won't be hard to auto-generate cases 5 and
00548    * 6 if hard-coded routines prove more efficient. */
00549   switch (n) {
00550     case 2:
00551       return inv2(a, b);
00552       break;
00553     case 3:
00554       return inv3(a, b);
00555       break;
00556     case 4:
00557       return inv4(a, b);
00558       break;
00559     default:
00560       /* Set up an augmented matrix M = [A I] */
00561       for (i = 0; i < n; i++) {
00562         for (j = 0; j < cols; j++) {
00563           if (j >= n) {
00564             if (j-n == i) {
00565               m[i*cols+j] = 1.0;
00566             } else {
00567               m[i*cols+j] = 0;
00568             }
00569           } else {
00570             m[i*cols+j] = a[i*n+j];
00571           }
00572         }
00573       }
00574 
00575       if ((res = rref(n, cols, m)) < 0) {
00576         /* Singular matrix! */
00577         return res;
00578       }
00579 
00580       /* Extract B from the augmented matrix M = [I inv(A)] */
00581       for (i = 0; i < n; i++) {
00582         for (j = n, k = 0; j < cols; j++, k++) {
00583           b[i*n+k] = m[i*cols+j];
00584         }
00585       }
00586 
00587       return 0;
00588       break;
00589   }
00590 }
00591 
00622 int matrix_pseudoinverse(u32 n, u32 m, const double *a, double *b) {
00623   if (n == m)
00624     return matrix_inverse(n, a, b);
00625   if (n > m)
00626     return matrix_ataiat(n, m, a, b);
00627   if (n < m)                                  /* n < m */
00628     return matrix_ataati(n, m, a, b);
00629   return -1;
00630 }
00631 
00647 inline int matrix_atwaiat(u32 n, u32 m, const double *a,
00648                           const double *w, double *b) {
00649   u32 i, j, k;
00650   double c[m*m], inv[m*m];
00651   /* Check to make sure we're doing the right operation */
00652   if (n <= m) return -1;
00653 
00654   /* The resulting matrix is symmetric, so compute both halves at
00655    * once */
00656   for (i = 0; i < m; i++)
00657     for (j = i; j < m; j++) {
00658       c[m*i + j] = 0;
00659       if (i == j) {
00660         /* If this is a diagonal element, just sum the squares of the
00661          * column of A weighted by the weighting vector. */
00662         for (k = 0; k < n; k++)
00663           c[m*i + j] += w[k]*a[m*k + j]*a[m*k + j];
00664       } else {
00665         /* Otherwise, assign both off-diagonal elements at once. */
00666         for (k = 0; k < n; k++)
00667           c[m*i + j] = c[m*j + i] = w[k]*a[m*k + j]*a[m*k + i];
00668         c[m*j + i] = c[m*i + j];
00669       }
00670     }
00671   if (matrix_inverse(m, c, inv) < 0) return -1;
00672   for (i = 0; i < m; i++)
00673     for (j = 0; j < n; j++) {
00674       b[n*i + j] = 0;
00675       for (k = 0; k < m; k++)
00676         b[n*i + j] += inv[n*i+k] * a[m*j + k];
00677     }
00678   return 0;
00679 }
00680 
00696 inline int matrix_atawati(u32 n, u32 m, const double *a,
00697                           const double *w, double *b) {
00698   u32 i, j, k;
00699   double c[m*m], inv[m*m];
00700   /* Check to make sure we're doing the right operation */
00701   if (n <= m) return -1;
00702 
00703   /* TODO(MP) -- implement! */
00704   /* The resulting matrix is symmetric, so compute both halves at
00705    * once */
00706   for (i = 0; i < m; i++)
00707     for (j = i; j < m; j++) {
00708       c[m*i + j] = 0;
00709       if (i == j) {
00710         /* If this is a diagonal element, just sum the squares of the
00711          * column of A weighted by the weighting vector. */
00712         for (k = 0; k < n; k++)
00713           c[m*i + j] += w[k]*a[m*k + j]*a[m*k + j];
00714       } else {
00715         /* Otherwise, assign both off-diagonal elements at once. */
00716         for (k = 0; k < n; k++)
00717           c[m*i + j] = c[m*j + i] = w[k]*a[m*k + j]*a[m*k + i];
00718         c[m*j + i] = c[m*i + j];
00719       }
00720     }
00721   if (matrix_inverse(m, c, inv) < 0) return -1;
00722   for (i = 0; i < m; i++)
00723     for (j = 0; j < n; j++) {
00724       b[n*i + j] = 0;
00725       for (k = 0; k < m; k++)
00726         b[n*i + j] += inv[n*i+k] * a[m*j + k];
00727     }
00728   return 0;
00729 }
00730 
00743 inline int matrix_ataiat(u32 n, u32 m, const double *a, double *b) {
00744   u32 i;
00745   double w[n];
00746   for (i = 0; i < n; i++) w[i] = 1;
00747   return matrix_atwaiat(n, m, a, w, b);
00748 }
00749 
00762 inline int matrix_ataati(u32 n, u32 m, const double *a, double *b) {
00763   u32 i;
00764   double w[n];
00765   for (i = 0; i < n; i++) w[i] = 1;
00766   return matrix_atawati(n, m, a, w, b);
00767 }
00768 
00782 inline void matrix_multiply(u32 n, u32 m, u32 p, const double *a,
00783                             const double *b, double *c) {
00784   u32 i, j, k;
00785   for (i = 0; i < n; i++)
00786     for (j = 0; j < p; j++) {
00787       c[p*i + j] = 0;
00788       for (k = 0; k < m; k++)
00789         c[p*i + j] += a[m*i+k] * b[p*k + j];
00790     }
00791 }
00792 
00805 void matrix_add_sc(u32 n, u32 m, const double *a,
00806                    const double *b, double gamma, double *c) {
00807   u32 i, j;
00808   for (i = 0; i < n; i++)
00809     for (j = 0; j < m; j++)
00810       c[m*i + j] = a[m*i + j] + gamma * b[m*i + j];
00811 }
00812 
00823 void matrix_transpose(u32 n, u32 m,
00824                       const double *a, double *b) {
00825   u32 i, j;
00826   for (i = 0; i < n; i++)
00827     for (j = 0; j < m; j++)
00828       b[n*j+i] = a[m*i+j];
00829 }
00830 
00840 void matrix_copy(u32 n, u32 m, const double *a,
00841                  double *b) {
00842   u32 i, j;
00843   for (i = 0; i < n; i++)
00844     for (j = 0; j < m; j++)
00845       b[m*i+j] = a[m*i+j];
00846 }
00847 
00848 /* \} */
00849 
00865 double vector_dot(u32 n, const double *a,
00866                   const double *b) {
00867   u32 i;
00868   double out = 0;
00869   for (i = 0; i < n; i++)
00870     out += a[i]*b[i];
00871   return out;
00872 }
00873 
00883 double vector_norm(u32 n, const double *a) {
00884   u32 i;
00885   double out = 0;
00886   for (i = 0; i < n; i++)
00887     out += a[i]*a[i];
00888   return sqrt(out);
00889 }
00890 
00899 double vector_mean(u32 n, const double *a) {
00900   u32 i;
00901   double out = 0;
00902   for (i = 0; i < n; i++)
00903     out += a[i];
00904   return out / n;
00905 }
00906 
00914 void vector_normalize(u32 n, double *a) {
00915   u32 i;
00916   double norm = vector_norm(n, a);
00917   for (i = 0; i < n; i++)
00918     a[i] /= norm;
00919 }
00920 
00932 void vector_add_sc(u32 n, const double *a,
00933                    const double *b, double gamma,
00934                    double *c) {
00935   u32 i;
00936   for (i = 0; i < n; i++)
00937     c[i] = a[i] + gamma * b[i];
00938 }
00939 
00949 void vector_add(u32 n, const double *a,
00950                 const double *b, double *c) {
00951   vector_add_sc(n, a, b, 1, c);
00952 }
00953 
00963 void vector_subtract(u32 n, const double *a,
00964                      const double *b, double *c) {
00965   vector_add_sc(n, a, b, -1, c);
00966 }
00967 
00977 void vector_cross(const double a[3], const double b[3],
00978                   double c[3]) {
00979   c[0] = a[1]*b[2] - a[2]*b[1];
00980   c[1] = a[2]*b[0] - a[0]*b[2];
00981   c[2] = a[0]*b[1] - a[1]*b[0];
00982 }
00983 
00984 /* \} */
00985 /* \} */
00986 


enu
Author(s): Mike Purvis
autogenerated on Fri Jan 3 2014 11:21:07