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

Linux Cross Reference
Tina6/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 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 General Public License for more details.
 14  *
 15  * You should have received a copy of the GNU 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  * ANY users of TINA who require exemption from the existing licence must
 20  * negotiate a new licence with Dr. Neil.A.Thacker, the sole agent for
 21  * the University of Manchester.
 22  *
 23  **********
 24  * 
 25  * Program :    TINA
 26  * File    :  $Source: /home/tina/cvs/tina-libs/tina/geometry/geomCurve_con_klmn.c,v $
 27  * Date    :  $Date: 2002/12/09 11:51:23 $
 28  * Version :  $Revision: 1.1.1.1 $
 29  * CVS Id  :  $Id: geomCurve_con_klmn.c,v 1.1.1.1 2002/12/09 11:51:23 cvstina Exp $
 30  *
 31  * Author  : Legacy TINA
 32  *
 33  * Notes : Bierman u-d form of scalar measurement filter
 34  *
 35  *********
 36 */
 37 
 38 #include "geomCurve_con_klmn.h"
 39 
 40 #if HAVE_CONFIG_H
 41   #include <config.h>
 42 #endif
 43 
 44 #include <math.h>
 45 #include <tina/sys/sysDef.h>
 46 #include <tina/sys/sysPro.h>
 47 #include <tina/math/mathDef.h>
 48 #include <tina/math/mathPro.h>
 49 #include <tina/geometry/geom_CurveDef.h>
 50 
 51 
 52 #define N 5
 53 
 54 /* Renamed from kalman by Julian to avoid compiler warning: 'kalman
 55  * static & extern' */
 56 static double klmn(double *x, double (*u)[5], double *d, double z, double *h, double var)
 57 {
 58     int     i, j;
 59     double  a[N], s[N], r[N], p[N], k[N], denom, sum, uij;
 60 
 61     for (i = 0; i < N; i++)
 62     {
 63         sum = 0.0;
 64         for (j = 0; j < i; j++)
 65             sum += u[j][i] * h[j];
 66         r[i] = sum + h[i];
 67     }
 68 
 69     for (i = 0; i < N; i++)
 70     {
 71         z += h[i] * x[i];
 72         s[i] = d[i] * r[i];
 73     }
 74 
 75     a[0] = s[0] * r[0] + var;
 76     a[0] = s[0] * r[0] + var;
 77     d[0] *= var / a[0];
 78     k[0] = s[0];
 79 
 80     for (j = 1; j < N; j++)
 81     {
 82         a[j] = a[j - 1] + s[j] * r[j];
 83         d[j] *= a[j - 1] / a[j];
 84         k[j] = s[j];
 85         p[j] = -r[j] / a[j - 1];
 86         for (i = 0; i < j; i++)
 87         {
 88             uij = u[i][j];
 89             u[i][j] += k[i] * p[j];
 90             k[i] += uij * s[j];
 91         }
 92     }
 93 
 94     denom = a[4];
 95     denom = 1.0 / denom;
 96 
 97     for (i = 0; i < N; i++)
 98         x[i] -= z * k[i] * denom;
 99 
100     return (z * z * denom);
101 }
102 
103 /**
104 
105 naive least squares (var unused)
106 **/
107 
108 double  conic_nlsq(Conic * conic, Conic_stat * stats, Vec2 p, double var)
109 {
110     double  z, h[5];
111     double  a, b, c, d, e, f;
112     double  px = vec2_x(p), py = vec2_y(p);
113 
114     a = conic->a;
115     b = conic->b;
116     c = conic->c;
117     d = conic->d;
118     e = conic->e;
119     f = conic->f;
120 
121     z = a * px * px + 2.0 * b * px * py + c * py * py +
122         2.0 * d * px + 2.0 * e * py + f;
123     h[0] = px * px - py * py;
124     h[1] = 2.0 * px * py;
125     h[2] = 2.0 * px;
126     h[3] = 2.0 * py;
127     h[4] = 1.0;
128     var = 1.0;                  /* var reset to unity */
129 
130     return (klmn(stats->x, stats->u, stats->d, z, h, var));
131 }
132 
133 /**
134 extended kalman filter
135 **/
136 
137 double  conic_ekf(Conic * conic, Conic_stat * stats, Vec2 p, double var)
138 {
139     double  z, h[5];
140     double  a, b, c, d, e, f;
141     double  px = vec2_x(p), py = vec2_y(p);
142     double  fx, fy;
143 
144     a = conic->a;
145     b = conic->b;
146     c = conic->c;
147     d = conic->d;
148     e = conic->e;
149     f = conic->f;
150 
151     z = a * px * px + 2.0 * b * px * py + c * py * py +
152         2.0 * d * px + 2.0 * e * py + f;
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     fx = 2.0 * (a * px + b * py + d);
159     fy = 2.0 * (b * px + c * py + e);
160     var *= (fx * fx + fy * fy);
161 
162     return (klmn(stats->x, stats->u, stats->d, z, h, var));
163 }
164 
165 /**
166 bias corrected kalman filter
167 **/
168 
169 double  conic_bckf(Conic * conic, Conic_stat * stats, Vec2 p, double var)
170 {
171     double  z, h[5];
172     double  a, b, c, d, e, f;
173     double  px = vec2_x(p), py = vec2_y(p);
174     double  fx, fy, t;
175 
176     a = conic->a;
177     b = conic->b;
178     c = conic->c;
179     d = conic->d;
180     e = conic->e;
181     f = conic->f;
182 
183     z = a * px * px + 2.0 * b * px * py + c * py * py +
184         2.0 * d * px + 2.0 * e * py + f;
185     fx = 2.0 * (a * px + b * py + d);
186     fy = 2.0 * (b * px + c * py + e);
187     var *= (fx * fx + fy * fy);
188     t = 2.0 * z / (fx * fx + fy * fy);
189 
190     h[0] = px * px - py * py;
191     h[1] = 2.0 * px * py;
192     h[2] = 2.0 * px;
193     h[3] = 2.0 * py;
194     h[4] = 1.0;
195 
196     h[0] -= t * (px * fx - py * fy);
197     h[1] -= t * (py * fx + px * fy);
198     h[2] -= t * fx;
199     h[3] -= t * fy;
200 
201     return (klmn(stats->x, stats->u, stats->d, z, h, var));
202 }
203 

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