~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
Tina4/src/geomstat/kalman/kalman.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 /**@(#)
  2 **/
  3 /**
  4 kalman.c:
  5 stationary scalar kalman filter for constraints
  6 **/
  7 
  8 #include <stdio.h>
  9 #include <math.h>
 10 #include <tina/sys.h>
 11 #include <tina/sysfuncs.h>
 12 #include <tina/math.h>
 13 #include <tina/mathfuncs.h>
 14 #include <tina/vision.h>
 15 #include <tina/visionfuncs.h>
 16 
 17 static Bool verbose = false;
 18 
 19 Bool    kalman_verbose_get(void)
 20 {
 21     return (verbose);
 22 }
 23 
 24 void    kalman_verbose_set(Bool newt)
 25 {
 26     verbose = newt;
 27 }
 28 
 29 void    vector2_free(Vector * v)
 30 {
 31     int     i;
 32 
 33     if (v == NULL)
 34         return;
 35     for (i = 0; i < v->n; i++)
 36         vector_free((Vector *) vector_getp(v, i));
 37     vector_free(v);
 38 }
 39 
 40 void    vector2_format(Vector * v)
 41 {
 42     int     i;
 43 
 44     vector_format(v);
 45     format("\n");
 46     for (i = 0; i < v->n; i++)
 47     {
 48         Vector *vi = (Vector *) vector_getp(v, i);
 49 
 50         if (vi != NULL)
 51         {
 52             format("%10d: ", i);
 53             vector_format(vi);
 54             format("\n");
 55         }
 56     }
 57 }
 58 
 59 static Vector *accum_vprod_1(Vector * x, Matrix * a, Vector * v)
 60 {
 61     int     i, j, m, n;
 62     double *xe, **ae, *ve;
 63 
 64     if (a == NULL || v == NULL)
 65         return (x);
 66 
 67     m = a->m;
 68     n = a->n;
 69     if (x == NULL)
 70         x = vector_alloc(m, double_v);
 71 
 72     xe = x->data;
 73     ae = a->el.double_v;
 74     ve = v->data;
 75 
 76     for (i = 0; i < m; i++)
 77         for (j = 0; j < n; j++)
 78             xe[i] += ae[i][j] * ve[j];
 79 
 80     return (x);
 81 }
 82 
 83 static Vector *accum_vprod_2(Vector * x, Matrix * a, Vector * v)
 84 {
 85     int     i, j, m;
 86     double *xe, **ae, *ve;
 87 
 88     if (a == NULL || v == NULL)
 89         return (x);
 90 
 91     m = a->m;
 92     if (x == NULL)
 93         x = vector_alloc(m, double_v);
 94 
 95     xe = x->data;
 96     ae = a->el.double_v;
 97     ve = v->data;
 98 
 99     for (i = 0; i < m; i++)
100     {
101         for (j = 0; j < i; j++)
102             xe[i] += ae[i][j] * ve[j];
103         for (j = i; j < m; j++)
104             xe[i] += ae[j][i] * ve[j];
105     }
106 
107     return (x);
108 }
109 
110 static Vector *accum_vprod_3(Vector * x, Matrix * a, Vector * v)
111 {
112     int     i, j, m, n;
113     double *xe, **ae, *ve;
114 
115     if (a == NULL || v == NULL)
116         return (x);
117 
118     m = a->m;
119     n = a->n;
120     if (x == NULL)
121         x = vector_alloc(n, double_v);
122 
123     xe = x->data;
124     ae = a->el.double_v;
125     ve = v->data;
126 
127     for (i = 0; i < n; i++)
128         for (j = 0; j < m; j++)
129             xe[i] += ae[j][i] * ve[j];
130 
131     return (x);
132 }
133 
134 static Vector *smatrix2_vprod(Matrix * a, Vector * v)
135 {
136     int     i, j;
137     int     m, n;
138     Vector *av;
139 
140     m = n = MIN(a->n, v->n);
141 
142     av = vector_alloc(m, ptr_v);
143     for (i = 0; i < m; i++)
144     {
145         Vector *avi = (Vector *) vector_getp(av, i);
146 
147         for (j = 0; j < i; j++)
148         {
149             Matrix *aij = (Matrix *) matrix_getp(a, i, j);
150             Vector *vj = (Vector *) vector_getp(v, j);
151 
152             avi = accum_vprod_1(avi, aij, vj);
153         }
154 
155         {
156             Matrix *aii = (Matrix *) matrix_getp(a, i, i);
157             Vector *vi = (Vector *) vector_getp(v, i);
158 
159             avi = accum_vprod_2(avi, aii, vi);
160         }
161 
162         for (j = i + 1; j < n; j++)
163         {
164             Matrix *aij = (Matrix *) matrix_getp(a, j, i);
165             Vector *vj = (Vector *) vector_getp(v, j);
166 
167             avi = accum_vprod_3(avi, aij, vj);
168         }
169         vector_putp((void *) avi, av, i);
170     }
171     return (av);
172 }
173 
174 static double vector2_dot(Vector * v1, Vector * v2)
175 {
176     int     i;
177     int     n;
178     double  v12 = 0.0;
179 
180     n = MIN(v1->n, v2->n);
181     for (i = 0; i < n; i++)
182         v12 += vector_dot((Vector *) vector_getp(v1, i),
183                           (Vector *) vector_getp(v2, i));
184     return (v12);
185 }
186 
187 static Vector *accum_vector_times(Vector * x, double k, Vector * v)
188 {
189     int     i;
190     int     n;
191     double *xe, *ve;
192 
193     if (k == 0.0 || v == NULL)
194         return (x);
195 
196     n = v->n;
197     if (x == NULL)
198         x = vector_alloc(n, double_v);
199 
200     xe = x->data;
201     ve = v->data;
202     for (i = 0; i < n; i++)
203         xe[i] += k * ve[i];
204 
205     return (x);
206 }
207 
208 
209 static void accum_vector2_times(Vector * x, double k, Vector * v)
210 {
211     int     i;
212     int     n;
213 
214     n = MIN(x->n, v->n);
215     for (i = 0; i < n; i++)
216     {
217         Vector *xi = (Vector *) vector_getp(x, i);
218         Vector *vi = (Vector *) vector_getp(v, i);
219 
220         xi = accum_vector_times(xi, k, vi);
221         vector_putp((void *) xi, x, i);
222     }
223 }
224 
225 static Matrix *accum_matrix_tensor(Matrix * a, double k, Vector * v, Vector * w)
226 {
227     int     i, j, m, n;
228     double **ae, *ve, *we;
229 
230     if (k == 0.0 || v == NULL || w == NULL)
231         return (a);
232 
233     m = v->n;
234     n = w->n;
235     if (a == NULL)
236         a = matrix_alloc(m, n, matrix_full, double_v);
237 
238     ae = a->el.double_v;
239     ve = v->data;
240     we = w->data;
241     for (i = 0; i < m; i++)
242         for (j = 0; j < n; j++)
243             ae[i][j] += k * ve[i] * we[j];
244     return (a);
245 }
246 
247 static Matrix *accum_matrix_symtensor(Matrix * a, double k, Vector * v)
248 {
249     int     i, j, n;
250     double **ae, *ve;
251 
252     if (k == 0.0 || v == NULL)
253         return (a);
254 
255     n = v->n;
256     if (a == NULL)
257         a = matrix_alloc(n, n, matrix_lower, double_v);
258 
259     ae = a->el.double_v;
260     ve = v->data;
261     for (i = 0; i < n; i++)
262         for (j = 0; j <= i; j++)
263             ae[i][j] += k * ve[i] * ve[j];
264     return (a);
265 }
266 
267 void    accum_smatrix2_symtensor(Matrix * a, double k, Vector * v)
268 {
269     int     i, j;
270     int     n;
271 
272     n = MIN(a->n, v->n);
273     for (i = 0; i < n; i++)
274     {
275         Vector *vi = (Vector *) vector_getp(v, i);
276 
277         for (j = 0; j < i; j++)
278         {
279             Matrix *aij = (Matrix *) matrix_getp(a, i, j);
280             Vector *vj = (Vector *) vector_getp(v, j);
281 
282             aij = accum_matrix_tensor(aij, k, vi, vj);
283             matrix_putp((void *) aij, a, i, j);
284         }
285 
286         {
287             Matrix *aii = (Matrix *) matrix_getp(a, i, i);
288 
289             aii = accum_matrix_symtensor(aii, k, vi);
290             matrix_putp((void *) aii, a, i, i);
291         }
292     }
293 }
294 
295 /**
296 imposes constraint with n active primitives as:
297 z+h[i0].x[i0]+...+h[i(n-1)].x[i(n-1)] = 0
298 **/
299 
300 static Bool kalman_error(double z, double hsh, double eps, int *pdf, double *res)
301 {
302     if (hsh < -eps * eps)
303     {
304         error("kalman: not positive indefinite", non_fatal);
305         *res = 1e10;
306         return (true);
307     } else if (fabs(hsh) < eps * eps)
308     {
309         if (fabs(z) > 2.0 * eps)
310         {
311             error("kalman: inconsistent", non_fatal);
312             *res = 1e10;
313         } else
314         {
315             error("kalman: dependent", warning);
316             *res = 0.0;
317             (*pdf)--;
318         }
319         return (true);
320     } else
321         return (false);
322 }
323 
324 double  kalman(State * state, double z, Vector * h, double eps, int *pdf)
325 {
326     Vector *sh;
327     double  hsh, res;
328 
329     sh = smatrix2_vprod(state->s, h);
330     hsh = vector2_dot(h, sh);
331     z += vector2_dot(state->x, h);
332 
333     if (kalman_verbose_get())
334         format("z = %e hsh = %e sigma = %e\n", z, hsh, sqrt(fabs(hsh)));
335 
336     if (kalman_error(z, hsh, eps, pdf, &res))
337     {
338         vector2_free(sh);
339         return (res);
340     }
341     accum_vector2_times(state->x, -z / hsh, sh);
342     accum_smatrix2_symtensor(state->s, -1.0 / hsh, sh);
343 
344     vector2_free(sh);
345     return (z * z / hsh);
346 }
347 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.