/* Routine to predict position and uncertainty at any time, given
* a PBASIS fit and a sigma matrix.
*/
double
predict_posn(PBASIS *pin,
double **covar,
OBSERVATION *obs,
double **sigxy) /*this holds xy error matrix*/
{
int i,j,t;
double *dx,*dy, distance;
dx=dvector(1,6);
dy=dvector(1,6);
/*using input time & obscode, put xy position into OBSERVATION struct*/
distance = kbo2d(pin,obs,
&(obs->thetax),dx,&(obs->thetay),dy);
/* project the covariance matrix */
/* skip if not desired */
if (sigxy!=NULL && covar!=NULL) {
sigxy[1][1]=sigxy[1][2]=sigxy[2][2]=0;
for (i=1; i<=6; i++) {
for (j=1; j<=6; j++) {
sigxy[1][1] += dx[i]*covar[i][j]*dx[j];
sigxy[1][2] += dx[i]*covar[i][j]*dy[j];
sigxy[2][2] += dy[i]*covar[i][j]*dy[j];
}
}
sigxy[2][1]=sigxy[1][2];
}
free_dvector(dx,1,6);
free_dvector(dy,1,6);
return distance;
}
void
fake_observation(PBASIS *p,
OBSERVATION *obs)
{
static long idum=-1;
float gasdev(long *idum);
/* seed the random number generator*/
if (idum<0) {
struct timeval tp;
gettimeofday(&tp,NULL);
idum = -tp.tv_usec;
}
kbo2d(p,obs,&(obs->thetax),NULL,&(obs->thetay),NULL);
/* Add measurement noise to the positions */
obs->thetax += obs->dthetax*gasdev(&idum);
obs->thetay += obs->dthetay*gasdev(&idum);
return;
}
