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

Linux Cross Reference
Tina4/src/vision/conic/con_klmn.c

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

  1 /**@(#)
  2 **/
  3 #include <math.h>
  4 #include <tina/sys.h>
  5 #include <tina/sysfuncs.h>
  6 #include <tina/math.h>
  7 #include <tina/mathfuncs.h>
  8 #include <tina/vision.h>
  9 #include <tina/visionfuncs.h>
 10 
 11 /**
 12 Bierman u-d form of scalar measurement filter
 13 **/
 14 
 15 #define N 5
 16 
 17 /* Renamed from kalman by Julian to avoid compiler warning: 'kalman
 18  * static & extern' */
 19 static double klmn(double *x, double (*u)[5], double *d, double z, double *h, double var)
 20 {
 21     int     i, j;
 22     double  a[N], s[N], r[N], p[N], k[N], denom, sum, uij;
 23 
 24     for (i = 0; i < N; i++)
 25     {
 26         sum = 0.0;
 27         for (j = 0; j < i; j++)
 28             sum += u[j][i] * h[j];
 29         r[i] = sum + h[i];
 30     }
 31 
 32     for (i = 0; i < N; i++)
 33     {
 34         z += h[i] * x[i];
 35         s[i] = d[i] * r[i];
 36     }
 37 
 38     a[0] = s[0] * r[0] + var;
 39     a[0] = s[0] * r[0] + var;
 40     d[0] *= var / a[0];
 41     k[0] = s[0];
 42 
 43     for (j = 1; j < N; j++)
 44     {
 45         a[j] = a[j - 1] + s[j] * r[j];
 46         d[j] *= a[j - 1] / a[j];
 47         k[j] = s[j];
 48         p[j] = -r[j] / a[j - 1];
 49         for (i = 0; i < j; i++)
 50         {
 51             uij = u[i][j];
 52             u[i][j] += k[i] * p[j];
 53             k[i] += uij * s[j];
 54         }
 55     }
 56 
 57     denom = a[4];
 58     denom = 1.0 / denom;
 59 
 60     for (i = 0; i < N; i++)
 61         x[i] -= z * k[i] * denom;
 62 
 63     return (z * z * denom);
 64 }
 65 
 66 /**
 67 
 68 naive least squares (var unused)
 69 **/
 70 
 71 double  conic_nlsq(Conic * conic, Conic_stat * stats, Vec2 p, double var)
 72 {
 73     double  z, h[5];
 74     double  a, b, c, d, e, f;
 75     double  px = vec2_x(p), py = vec2_y(p);
 76 
 77     a = conic->a;
 78     b = conic->b;
 79     c = conic->c;
 80     d = conic->d;
 81     e = conic->e;
 82     f = conic->f;
 83 
 84     z = a * px * px + 2.0 * b * px * py + c * py * py +
 85         2.0 * d * px + 2.0 * e * py + f;
 86     h[0] = px * px - py * py;
 87     h[1] = 2.0 * px * py;
 88     h[2] = 2.0 * px;
 89     h[3] = 2.0 * py;
 90     h[4] = 1.0;
 91     var = 1.0;                  /* var reset to unity */
 92 
 93     return (klmn(stats->x, stats->u, stats->d, z, h, var));
 94 }
 95 
 96 /**
 97 extended kalman filter
 98 **/
 99 
100 double  conic_ekf(Conic * conic, Conic_stat * stats, Vec2 p, double var)
101 {
102     double  z, h[5];
103     double  a, b, c, d, e, f;
104     double  px = vec2_x(p), py = vec2_y(p);
105     double  fx, fy;
106 
107     a = conic->a;
108     b = conic->b;
109     c = conic->c;
110     d = conic->d;
111     e = conic->e;
112     f = conic->f;
113 
114     z = a * px * px + 2.0 * b * px * py + c * py * py +
115         2.0 * d * px + 2.0 * e * py + f;
116     h[0] = px * px - py * py;
117     h[1] = 2.0 * px * py;
118     h[2] = 2.0 * px;
119     h[3] = 2.0 * py;
120     h[4] = 1.0;
121     fx = 2.0 * (a * px + b * py + d);
122     fy = 2.0 * (b * px + c * py + e);
123     var *= (fx * fx + fy * fy);
124 
125     return (klmn(stats->x, stats->u, stats->d, z, h, var));
126 }
127 
128 /**
129 bias corrected kalman filter
130 **/
131 
132 double  conic_bckf(Conic * conic, Conic_stat * stats, Vec2 p, double var)
133 {
134     double  z, h[5];
135     double  a, b, c, d, e, f;
136     double  px = vec2_x(p), py = vec2_y(p);
137     double  fx, fy, t;
138 
139     a = conic->a;
140     b = conic->b;
141     c = conic->c;
142     d = conic->d;
143     e = conic->e;
144     f = conic->f;
145 
146     z = a * px * px + 2.0 * b * px * py + c * py * py +
147         2.0 * d * px + 2.0 * e * py + f;
148     fx = 2.0 * (a * px + b * py + d);
149     fy = 2.0 * (b * px + c * py + e);
150     var *= (fx * fx + fy * fy);
151     t = 2.0 * z / (fx * fx + fy * fy);
152 
153     h[0] = px * px - py * py;
154     h[1] = 2.0 * px * py;
155     h[2] = 2.0 * px;
156     h[3] = 2.0 * py;
157     h[4] = 1.0;
158 
159     h[0] -= t * (px * fx - py * fy);
160     h[1] -= t * (py * fx + px * fy);
161     h[2] -= t * fx;
162     h[3] -= t * fy;
163 
164     return (klmn(stats->x, stats->u, stats->d, z, h, var));
165 }
166 

~ [ 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.