#include <stdio.h>#include <math.h>#include <tina/sys.h>#include <tina/sysfuncs.h>#include <tina/math.h>#include <tina/mathfuncs.h>#include <tina/vision.h>#include <tina/visionfuncs.h>Include dependency graph for kalman.c:
Go to the source code of this file.
Functions | |
| Bool | kalman_verbose_get (void) |
| void | kalman_verbose_set (Bool newt) |
| void | vector2_free (Vector *v) |
| void | vector2_format (Vector *v) |
| Vector * | accum_vprod_1 (Vector *x, Matrix *a, Vector *v) |
| Vector * | accum_vprod_2 (Vector *x, Matrix *a, Vector *v) |
| Vector * | accum_vprod_3 (Vector *x, Matrix *a, Vector *v) |
| Vector * | smatrix2_vprod (Matrix *a, Vector *v) |
| double | vector2_dot (Vector *v1, Vector *v2) |
| Vector * | accum_vector_times (Vector *x, double k, Vector *v) |
| void | accum_vector2_times (Vector *x, double k, Vector *v) |
| Matrix * | accum_matrix_tensor (Matrix *a, double k, Vector *v, Vector *w) |
| Matrix * | accum_matrix_symtensor (Matrix *a, double k, Vector *v) |
| void | accum_smatrix2_symtensor (Matrix *a, double k, Vector *v) |
| Bool | kalman_error (double z, double hsh, double eps, int *pdf, double *res) |
| double | kalman (State *state, double z, Vector *h, double eps, int *pdf) |
Variables | |
| Bool | verbose = false |
|
||||||||||||||||
|
Definition at line 247 of file kalman.c. References vector::data, Matrix, matrix_alloc(), matrix_lower, vector::n, and Vector. Referenced by accum_smatrix2_symtensor().
00248 {
00249 int i, j, n;
00250 double **ae, *ve;
00251
00252 if (k == 0.0 || v == NULL)
00253 return (a);
00254
00255 n = v->n;
00256 if (a == NULL)
00257 a = matrix_alloc(n, n, matrix_lower, double_v);
00258
00259 ae = a->el.double_v;
00260 ve = v->data;
00261 for (i = 0; i < n; i++)
00262 for (j = 0; j <= i; j++)
00263 ae[i][j] += k * ve[i] * ve[j];
00264 return (a);
00265 }
|
Here is the call graph for this function:
|
||||||||||||||||||||
|
Definition at line 225 of file kalman.c. References vector::data, Matrix, matrix_alloc(), matrix_full, vector::n, and Vector. Referenced by accum_smatrix2_symtensor().
00226 {
00227 int i, j, m, n;
00228 double **ae, *ve, *we;
00229
00230 if (k == 0.0 || v == NULL || w == NULL)
00231 return (a);
00232
00233 m = v->n;
00234 n = w->n;
00235 if (a == NULL)
00236 a = matrix_alloc(m, n, matrix_full, double_v);
00237
00238 ae = a->el.double_v;
00239 ve = v->data;
00240 we = w->data;
00241 for (i = 0; i < m; i++)
00242 for (j = 0; j < n; j++)
00243 ae[i][j] += k * ve[i] * we[j];
00244 return (a);
00245 }
|
Here is the call graph for this function:
|
||||||||||||||||
|
Definition at line 267 of file kalman.c. References accum_matrix_symtensor(), accum_matrix_tensor(), Matrix, matrix_getp(), matrix_putp(), MIN, vector::n, Vector, and vector_getp(). Referenced by kalman().
00268 {
00269 int i, j;
00270 int n;
00271
00272 n = MIN(a->n, v->n);
00273 for (i = 0; i < n; i++)
00274 {
00275 Vector *vi = (Vector *) vector_getp(v, i);
00276
00277 for (j = 0; j < i; j++)
00278 {
00279 Matrix *aij = (Matrix *) matrix_getp(a, i, j);
00280 Vector *vj = (Vector *) vector_getp(v, j);
00281
00282 aij = accum_matrix_tensor(aij, k, vi, vj);
00283 matrix_putp((void *) aij, a, i, j);
00284 }
00285
00286 {
00287 Matrix *aii = (Matrix *) matrix_getp(a, i, i);
00288
00289 aii = accum_matrix_symtensor(aii, k, vi);
00290 matrix_putp((void *) aii, a, i, i);
00291 }
00292 }
00293 }
|
Here is the call graph for this function:
|
||||||||||||||||
|
Definition at line 209 of file kalman.c. References accum_vector_times(), MIN, vector::n, Vector, vector_getp(), and vector_putp(). Referenced by kalman().
00210 {
00211 int i;
00212 int n;
00213
00214 n = MIN(x->n, v->n);
00215 for (i = 0; i < n; i++)
00216 {
00217 Vector *xi = (Vector *) vector_getp(x, i);
00218 Vector *vi = (Vector *) vector_getp(v, i);
00219
00220 xi = accum_vector_times(xi, k, vi);
00221 vector_putp((void *) xi, x, i);
00222 }
00223 }
|
Here is the call graph for this function:
|
||||||||||||||||
|
Definition at line 187 of file kalman.c. References vector::data, vector::n, Vector, and vector_alloc(). Referenced by accum_vector2_times().
00188 {
00189 int i;
00190 int n;
00191 double *xe, *ve;
00192
00193 if (k == 0.0 || v == NULL)
00194 return (x);
00195
00196 n = v->n;
00197 if (x == NULL)
00198 x = vector_alloc(n, double_v);
00199
00200 xe = x->data;
00201 ve = v->data;
00202 for (i = 0; i < n; i++)
00203 xe[i] += k * ve[i];
00204
00205 return (x);
00206 }
|
Here is the call graph for this function:
|
||||||||||||||||
|
Definition at line 59 of file kalman.c. References vector::data, Matrix, Vector, and vector_alloc(). Referenced by smatrix2_vprod().
00060 {
00061 int i, j, m, n;
00062 double *xe, **ae, *ve;
00063
00064 if (a == NULL || v == NULL)
00065 return (x);
00066
00067 m = a->m;
00068 n = a->n;
00069 if (x == NULL)
00070 x = vector_alloc(m, double_v);
00071
00072 xe = x->data;
00073 ae = a->el.double_v;
00074 ve = v->data;
00075
00076 for (i = 0; i < m; i++)
00077 for (j = 0; j < n; j++)
00078 xe[i] += ae[i][j] * ve[j];
00079
00080 return (x);
00081 }
|
Here is the call graph for this function:
|
||||||||||||||||
|
Definition at line 83 of file kalman.c. References vector::data, Matrix, Vector, and vector_alloc(). Referenced by smatrix2_vprod().
00084 {
00085 int i, j, m;
00086 double *xe, **ae, *ve;
00087
00088 if (a == NULL || v == NULL)
00089 return (x);
00090
00091 m = a->m;
00092 if (x == NULL)
00093 x = vector_alloc(m, double_v);
00094
00095 xe = x->data;
00096 ae = a->el.double_v;
00097 ve = v->data;
00098
00099 for (i = 0; i < m; i++)
00100 {
00101 for (j = 0; j < i; j++)
00102 xe[i] += ae[i][j] * ve[j];
00103 for (j = i; j < m; j++)
00104 xe[i] += ae[j][i] * ve[j];
00105 }
00106
00107 return (x);
00108 }
|
Here is the call graph for this function:
|
||||||||||||||||
|
Definition at line 110 of file kalman.c. References vector::data, Matrix, Vector, and vector_alloc(). Referenced by smatrix2_vprod().
00111 {
00112 int i, j, m, n;
00113 double *xe, **ae, *ve;
00114
00115 if (a == NULL || v == NULL)
00116 return (x);
00117
00118 m = a->m;
00119 n = a->n;
00120 if (x == NULL)
00121 x = vector_alloc(n, double_v);
00122
00123 xe = x->data;
00124 ae = a->el.double_v;
00125 ve = v->data;
00126
00127 for (i = 0; i < n; i++)
00128 for (j = 0; j < m; j++)
00129 xe[i] += ae[j][i] * ve[j];
00130
00131 return (x);
00132 }
|
Here is the call graph for this function:
|
||||||||||||||||||||||||
|
Definition at line 324 of file kalman.c. References accum_smatrix2_symtensor(), accum_vector2_times(), format(), kalman_error(), kalman_verbose_get(), state::s, sh, smatrix2_vprod(), sqrt(), State, Vector, vector2_dot(), vector2_free(), and state::x.
00325 {
00326 Vector *sh;
00327 double hsh, res;
00328
00329 sh = smatrix2_vprod(state->s, h);
00330 hsh = vector2_dot(h, sh);
00331 z += vector2_dot(state->x, h);
00332
00333 if (kalman_verbose_get())
00334 format("z = %e hsh = %e sigma = %e\n", z, hsh, sqrt(fabs(hsh)));
00335
00336 if (kalman_error(z, hsh, eps, pdf, &res))
00337 {
00338 vector2_free(sh);
00339 return (res);
00340 }
00341 accum_vector2_times(state->x, -z / hsh, sh);
00342 accum_smatrix2_symtensor(state->s, -1.0 / hsh, sh);
00343
00344 vector2_free(sh);
00345 return (z * z / hsh);
00346 }
|
Here is the call graph for this function:
|
||||||||||||||||||||||||
|
imposes constraint with n active primitives as: z+h[i0].x[i0]+...+h[i(n-1)].x[i(n-1)] = 0 Definition at line 300 of file kalman.c. References Bool, error(), non_fatal, and warning. Referenced by kalman().
00301 {
00302 if (hsh < -eps * eps)
00303 {
00304 error("kalman: not positive indefinite", non_fatal);
00305 *res = 1e10;
00306 return (true);
00307 } else if (fabs(hsh) < eps * eps)
00308 {
00309 if (fabs(z) > 2.0 * eps)
00310 {
00311 error("kalman: inconsistent", non_fatal);
00312 *res = 1e10;
00313 } else
00314 {
00315 error("kalman: dependent", warning);
00316 *res = 0.0;
00317 (*pdf)--;
00318 }
00319 return (true);
00320 } else
00321 return (false);
00322 }
|
Here is the call graph for this function:
|
|
Definition at line 19 of file kalman.c. Referenced by kalman().
00020 {
00021 return (verbose);
00022 }
|
|
|
Definition at line 24 of file kalman.c.
00025 {
00026 verbose = newt;
00027 }
|
|
||||||||||||
|
Definition at line 134 of file kalman.c. References accum_vprod_1(), accum_vprod_2(), accum_vprod_3(), Matrix, matrix_getp(), MIN, vector::n, Vector, vector_alloc(), vector_getp(), and vector_putp(). Referenced by kalman().
00135 {
00136 int i, j;
00137 int m, n;
00138 Vector *av;
00139
00140 m = n = MIN(a->n, v->n);
00141
00142 av = vector_alloc(m, ptr_v);
00143 for (i = 0; i < m; i++)
00144 {
00145 Vector *avi = (Vector *) vector_getp(av, i);
00146
00147 for (j = 0; j < i; j++)
00148 {
00149 Matrix *aij = (Matrix *) matrix_getp(a, i, j);
00150 Vector *vj = (Vector *) vector_getp(v, j);
00151
00152 avi = accum_vprod_1(avi, aij, vj);
00153 }
00154
00155 {
00156 Matrix *aii = (Matrix *) matrix_getp(a, i, i);
00157 Vector *vi = (Vector *) vector_getp(v, i);
00158
00159 avi = accum_vprod_2(avi, aii, vi);
00160 }
00161
00162 for (j = i + 1; j < n; j++)
00163 {
00164 Matrix *aij = (Matrix *) matrix_getp(a, j, i);
00165 Vector *vj = (Vector *) vector_getp(v, j);
00166
00167 avi = accum_vprod_3(avi, aij, vj);
00168 }
00169 vector_putp((void *) avi, av, i);
00170 }
00171 return (av);
00172 }
|
Here is the call graph for this function:
|
||||||||||||
|
Definition at line 174 of file kalman.c. References MIN, vector::n, Vector, vector_dot(), and vector_getp(). Referenced by kalman().
00175 {
00176 int i;
00177 int n;
00178 double v12 = 0.0;
00179
00180 n = MIN(v1->n, v2->n);
00181 for (i = 0; i < n; i++)
00182 v12 += vector_dot((Vector *) vector_getp(v1, i),
00183 (Vector *) vector_getp(v2, i));
00184 return (v12);
00185 }
|
Here is the call graph for this function:
|
|
Definition at line 40 of file kalman.c. References format(), vector::n, Vector, vector_format(), and vector_getp().
00041 {
00042 int i;
00043
00044 vector_format(v);
00045 format("\n");
00046 for (i = 0; i < v->n; i++)
00047 {
00048 Vector *vi = (Vector *) vector_getp(v, i);
00049
00050 if (vi != NULL)
00051 {
00052 format("%10d: ", i);
00053 vector_format(vi);
00054 format("\n");
00055 }
00056 }
00057 }
|
Here is the call graph for this function:
|
|
Definition at line 29 of file kalman.c. References vector::n, Vector, vector_free(), and vector_getp(). Referenced by kalman().
00030 {
00031 int i;
00032
00033 if (v == NULL)
00034 return;
00035 for (i = 0; i < v->n; i++)
00036 vector_free((Vector *) vector_getp(v, i));
00037 vector_free(v);
00038 }
|
Here is the call graph for this function:
|
|
kalman.c: stationary scalar kalman filter for constraints Definition at line 17 of file kalman.c. Referenced by kalman_verbose_get(), and kalman_verbose_set(). |
1.3.6