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

Linux Cross Reference
Tina5/tina-libs/tina/geometry/geomCurve_con_klmn.c

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

  1 /**********
  2  * 
  3  * This file is part of the TINA Open Source Image Analysis Environment
  4  * henceforth known as TINA
  5  *
  6  * TINA is free software; you can redistribute it and/or modify
  7  * it under the terms of the GNU Lesser General Public License as 
  8  * published by the Free Software Foundation.
  9  *
 10  * TINA is distributed in the hope that it will be useful,
 11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13  * GNU Lesser General Public License for more details.
 14  *
 15  * You should have received a copy of the GNU Lesser General Public License
 16  * along with TINA; if not, write to the Free Software Foundation, Inc., 
 17  * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 18  *
 19  **********
 20  * 
 21  * Program :    TINA
 22  * File    :  $Source: /home/tina/cvs/tina-libs/tina/geometry/geomCurve_con_klmn.c,v $
 23  * Date    :  $Date: 2002/12/09 11:51:23 $
 24  * Version :  $Revision: 1.1.1.1 $
 25  * CVS Id  :  $Id: geomCurve_con_klmn.c,v 1.1.1.1 2002/12/09 11:51:23 cvstina Exp $
 26  *
 27  * Author  : Legacy TINA
 28  *
 29  * Notes : Bierman u-d form of scalar measurement filter
 30  *
 31  *********
 32 */
 33 
 34 #include "geomCurve_con_klmn.h"
 35 
 36 #if HAVE_CONFIG_H
 37   #include <config.h>
 38 #endif
 39 
 40 #include <math.h>
 41 #include <tina/sys/sysDef.h>
 42 #include <tina/sys/sysPro.h>
 43 #include <tina/math/mathDef.h>
 44 #include <tina/math/mathPro.h>
 45 #include <tina/geometry/geom_CurveDef.h>
 46 
 47 
 48 #define N 5
 49 
 50 /* Renamed from kalman by Julian to avoid compiler warning: 'kalman
 51  * static & extern' */
 52 static double klmn(double *x, double (*u)[5], double *d, double z, double *h, double var)
 53 {
 54     int     i, j;
 55     double  a[N], s[N], r[N], p[N], k[N], denom, sum, uij;
 56 
 57     for (i = 0; i < N; i++)
 58     {
 59         sum = 0.0;
 60         for (j = 0; j < i; j++)
 61             sum += u[j][i] * h[j];
 62         r[i] = sum + h[i];
 63     }
 64 
 65     for (i = 0; i < N; i++)
 66     {
 67         z += h[i] * x[i];
 68         s[i] = d[i] * r[i];
 69     }
 70 
 71     a[0] = s[0] * r[0] + var;
 72     a[0] = s[0] * r[0] + var;
 73     d[0] *= var / a[0];
 74     k[0] = s[0];
 75 
 76     for (j = 1; j < N; j++)
 77     {
 78         a[j] = a[j - 1] + s[j] * r[j];
 79         d[j] *= a[j - 1] / a[j];
 80         k[j] = s[j];
 81         p[j] = -r[j] / a[j - 1];
 82         for (i = 0; i < j; i++)
 83         {
 84             uij = u[i][j];
 85             u[i][j] += k[i] * p[j];
 86             k[i] += uij * s[j];
 87         }
 88     }
 89 
 90     denom = a[4];
 91     denom = 1.0 / denom;
 92 
 93     for (i = 0; i < N; i++)
 94         x[i] -= z * k[i] * denom;
 95 
 96     return (z * z * denom);
 97 }
 98 
 99 /**
100 
101 naive least squares (var unused)
102 **/
103 
104 double  conic_nlsq(Conic * conic, Conic_stat * stats, Vec2 p, double var)
105 {
106     double  z, h[5];
107     double  a, b, c, d, e, f;
108     double  px = vec2_x(p), py = vec2_y(p);
109 
110     a = conic->a;
111     b = conic->b;
112     c = conic->c;
113     d = conic->d;
114     e = conic->e;
115     f = conic->f;
116 
117     z = a * px * px + 2.0 * b * px * py + c * py * py +
118         2.0 * d * px + 2.0 * e * py + f;
119     h[0] = px * px - py * py;
120     h[1] = 2.0 * px * py;
121     h[2] = 2.0 * px;
122     h[3] = 2.0 * py;
123     h[4] = 1.0;
124     var = 1.0;                  /* var reset to unity */
125 
126     return (klmn(stats->x, stats->u, stats->d, z, h, var));
127 }
128 
129 /**
130 extended kalman filter
131 **/
132 
133 double  conic_ekf(Conic * conic, Conic_stat * stats, Vec2 p, double var)
134 {
135     double  z, h[5];
136     double  a, b, c, d, e, f;
137     double  px = vec2_x(p), py = vec2_y(p);
138     double  fx, fy;
139 
140     a = conic->a;
141     b = conic->b;
142     c = conic->c;
143     d = conic->d;
144     e = conic->e;
145     f = conic->f;
146 
147     z = a * px * px + 2.0 * b * px * py + c * py * py +
148         2.0 * d * px + 2.0 * e * py + f;
149     h[0] = px * px - py * py;
150     h[1] = 2.0 * px * py;
151     h[2] = 2.0 * px;
152     h[3] = 2.0 * py;
153     h[4] = 1.0;
154     fx = 2.0 * (a * px + b * py + d);
155     fy = 2.0 * (b * px + c * py + e);
156     var *= (fx * fx + fy * fy);
157 
158     return (klmn(stats->x, stats->u, stats->d, z, h, var));
159 }
160 
161 /**
162 bias corrected kalman filter
163 **/
164 
165 double  conic_bckf(Conic * conic, Conic_stat * stats, Vec2 p, double var)
166 {
167     double  z, h[5];
168     double  a, b, c, d, e, f;
169     double  px = vec2_x(p), py = vec2_y(p);
170     double  fx, fy, t;
171 
172     a = conic->a;
173     b = conic->b;
174     c = conic->c;
175     d = conic->d;
176     e = conic->e;
177     f = conic->f;
178 
179     z = a * px * px + 2.0 * b * px * py + c * py * py +
180         2.0 * d * px + 2.0 * e * py + f;
181     fx = 2.0 * (a * px + b * py + d);
182     fy = 2.0 * (b * px + c * py + e);
183     var *= (fx * fx + fy * fy);
184     t = 2.0 * z / (fx * fx + fy * fy);
185 
186     h[0] = px * px - py * py;
187     h[1] = 2.0 * px * py;
188     h[2] = 2.0 * px;
189     h[3] = 2.0 * py;
190     h[4] = 1.0;
191 
192     h[0] -= t * (px * fx - py * fy);
193     h[1] -= t * (py * fx + px * fy);
194     h[2] -= t * fx;
195     h[3] -= t * fy;
196 
197     return (klmn(stats->x, stats->u, stats->d, z, h, var));
198 }
199 

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