git.gag.com Git - fw/altos/blob - src/kalman/matrix.5c

   1 /*
   2  * Copyright © 2011 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 namespace matrix {
  20         public typedef real[*] vec_t;
  21         public typedef real[*,*] mat_t;
  22
  23         public mat_t transpose(mat_t m) {
  24                 int[2] d = dims(m);
  25                 return (real[d[1],d[0]]) { [i,j] = m[j,i] };
  26         }
  27
  28         public void print_mat(string name, mat_t m) {
  29                 int[2]  d = dims(m);
  30                 printf ("%s {\n", name);
  31                 for (int y = 0; y < d[0]; y++) {
  32                         for (int x = 0; x < d[1]; x++)
  33                                 printf (" %14.8f", m[y,x]);
  34                         printf ("\n");
  35                 }
  36                 printf ("}\n");
  37         }
  38
  39         public void print_vec(string name, vec_t v) {
  40                 int d = dim(v);
  41                 printf ("%s {", name);
  42                 for (int x = 0; x < d; x++)
  43                         printf (" %14.8f", v[x]);
  44                 printf (" }\n");
  45         }
  46
  47         public mat_t multiply(mat_t a, mat_t b) {
  48                 int[2] da = dims(a);
  49                 int[2] db = dims(b);
  50
  51                 assert(da[1] == db[0], "invalid matrix dimensions");
  52
  53                 real dot(int rx, int ry) {
  54                         real    r = 0.0;
  55                         for (int i = 0; i < da[1]; i++)
  56                                 r += a[ry, i] * b[i, rx];
  57                         return imprecise(r);
  58                 }
  59
  60                 mat_t r = (real[da[0], db[1]]) { [ry,rx] = dot(rx,ry) };
  61                 return r;
  62         }
  63
  64         public mat_t multiply_mat_val(mat_t m, real value) {
  65                 int[2] d = dims(m);
  66                 for (int j = 0; j < d[1]; j++)
  67                         for (int i = 0; i < d[0]; i++)
  68                                 m[i,j] *= value;
  69                 return m;
  70         }
  71
  72         public mat_t add(mat_t a, mat_t b) {
  73                 int[2]  da = dims(a);
  74                 int[2]  db = dims(b);
  75
  76                 assert(da[0] == db[0] && da[1] == db[1], "mismatching dim in plus");
  77                 return (real[da[0], da[1]]) { [y,x] = a[y,x] + b[y,x] };
  78         }
  79
  80         public mat_t subtract(mat_t a, mat_t b) {
  81                 int[2]  da = dims(a);
  82                 int[2]  db = dims(b);
  83
  84                 assert(da[0] == db[0] && da[1] == db[1], "mismatching dim in minus");
  85                 return (real[da[0], da[1]]) { [y,x] = a[y,x] - b[y,x] };
  86         }
  87
  88         public mat_t inverse(mat_t m) {
  89                 int[2] d = dims(m);
  90
  91                 real[1,1] inverse_1(real[1,1] m) {
  92                         return (real[1,1]) { { 1/m[0,0] } };
  93                 }
  94
  95                 if (d[0] == 1 && d[1] == 1)
  96                         return inverse_1(m);
  97
  98                 real[2,2] inverse_2(real[2,2] m) {
  99                         real    a = m[0,0], b = m[0,1];
 100                         real    c = m[1,0], d = m[1,1];
 101                         real det = a * d - b * c;
 102                         return multiply_mat_val((real[2,2]) {
 103                                         { d, -b }, { -c, a } }, 1/det);
 104                 }
 105
 106                 if (d[0] == 2 && d[1] == 2)
 107                         return inverse_2(m);
 108
 109                 real[3,3] inverse_3(real[3,3] m) {
 110                         real    a = m[0,0], b = m[0,1], c = m[0, 2];
 111                         real    d = m[1,0], e = m[1,1], f = m[1, 2];
 112                         real    g = m[2,0], h = m[2,1], k = m[2, 2];
 113                         real    Z = a*(e*k-f*h) + b*(f*g - d*k) + c*(d*h-e*g);
 114                         real    A = (e*k-f*h), B = (c*h-b*k), C=(b*f-c*e);
 115                         real    D = (f*g-d*k), E = (a*k-c*g), F=(c*d-a*f);
 116                         real    G = (d*h-e*g), H = (b*g-a*h), K=(a*e-b*d);
 117                         return multiply_mat_val((real[3,3]) {
 118                                         { A, B, C }, { D, E, F }, { G, H, K }},
 119                                 1/Z);
 120                 }
 121
 122                 if (d[0] == 3 && d[1] == 3)
 123                         return inverse_3(m);
 124                 assert(false, "cannot invert %v\n", d);
 125                 return m;
 126         }
 127
 128         public mat_t identity(int d) {
 129                 return (real[d,d]) { [i,j] = (i == j) ? 1 : 0 };
 130         }
 131
 132         public vec_t vec_subtract(vec_t a, vec_t b) {
 133                 int     da = dim(a);
 134                 int     db = dim(b);
 135
 136                 assert(da == db, "mismatching dim in minus");
 137                 return (real[da]) { [x] = a[x] - b[x] };
 138         }
 139
 140         public vec_t vec_add(vec_t a, vec_t b) {
 141                 int     da = dim(a);
 142                 int     db = dim(b);
 143
 144                 assert(da == db, "mismatching dim in plus");
 145                 return (real[da]) { [x] = a[x] + b[x] };
 146         }
 147
 148         public vec_t multiply_vec_mat(vec_t v, mat_t m) {
 149                 mat_t r2 = matrix::multiply((real[dim(v),1]) { [y,x] = v[y] }, m);
 150                 return (real[dim(v)]) { [y] = r2[y,0] };
 151         }
 152
 153         public vec_t multiply_mat_vec(mat_t m, vec_t v) {
 154                 mat_t r2 = matrix::multiply(m, (real[dim(v), 1]) { [y,x] = v[y] });
 155                 int[2] d = dims(m);
 156                 return (real[d[0]]) { [y] = r2[y,0] };
 157         }
 158 }