From 0ab91a6e9374f190c049a5d8ea1319b9b37529c1 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 15 Apr 2018 17:43:12 -0400 Subject: Move things into attic and update Makefile to do dependnencies. --- attic/dave/AffineSolve | Bin 0 -> 40104 bytes attic/dave/AffineSolve.c | 783 +++++++++++++ attic/dave/AffineSolve.c.CHARLES | 643 +++++++++++ attic/dave/HMD_normals.csv | 32 + attic/dave/HMD_points.csv | 32 + attic/dave/Makefile | 21 + attic/dave/OrthoPlot.c | 451 ++++++++ attic/dave/dclapack_test | Bin 0 -> 12960 bytes attic/dave/dclapack_test.c | 34 + attic/dave/errors.txt | 2141 ++++++++++++++++++++++++++++++++++++ attic/dave/fileutil.c | 133 +++ attic/dave/fileutil.h | 35 + attic/dave/kalman_filter.c | 72 ++ attic/dave/kalman_filter.h | 116 ++ attic/dave/main.c | 29 + attic/dave/olddata/HMD_normals.csv | 32 + attic/dave/olddata/HMD_points.csv | 32 + attic/dave/olddata/ptinfo.csv | 88 ++ attic/dave/ptinfo.csv | 69 ++ 19 files changed, 4743 insertions(+) create mode 100755 attic/dave/AffineSolve create mode 100644 attic/dave/AffineSolve.c create mode 100644 attic/dave/AffineSolve.c.CHARLES create mode 100644 attic/dave/HMD_normals.csv create mode 100644 attic/dave/HMD_points.csv create mode 100644 attic/dave/Makefile create mode 100644 attic/dave/OrthoPlot.c create mode 100755 attic/dave/dclapack_test create mode 100644 attic/dave/dclapack_test.c create mode 100644 attic/dave/errors.txt create mode 100644 attic/dave/fileutil.c create mode 100644 attic/dave/fileutil.h create mode 100644 attic/dave/kalman_filter.c create mode 100644 attic/dave/kalman_filter.h create mode 100644 attic/dave/main.c create mode 100644 attic/dave/olddata/HMD_normals.csv create mode 100644 attic/dave/olddata/HMD_points.csv create mode 100644 attic/dave/olddata/ptinfo.csv create mode 100644 attic/dave/ptinfo.csv (limited to 'attic') diff --git a/attic/dave/AffineSolve b/attic/dave/AffineSolve new file mode 100755 index 0000000..bd93cbd Binary files /dev/null and b/attic/dave/AffineSolve differ diff --git a/attic/dave/AffineSolve.c b/attic/dave/AffineSolve.c new file mode 100644 index 0000000..685062e --- /dev/null +++ b/attic/dave/AffineSolve.c @@ -0,0 +1,783 @@ +// +// main.c +// Aff +// Created by user on 3/2/17. +// Copyright © 2017 user. All rights reserved. +// + +#include +#include +#include +#include +#include "dclapack.h" +#include +#define RegisterDriver(a,b) +#include "poser_daveortho.c" +#define LH_ID 0 +#define NUM_HMD 32 +#define INDIR "full_test_triangle_on_floor/" + +#define MAX_POINTS SENSORS_PER_OBJECT +//#define _ABS(a) ( (a)<=0 ? -(a) : (a) ) +#define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f ) +#define RANDF ( (float)rand() / (float)RAND_MAX ) +#define PI 3.14159265358979323846264 + +#define STEP_SIZE_ROT 1.0 +#define STEP_SIZE_POS 1.0 +#define FALLOFF 0.99999 +#define NITER 2000000 +#define TOO_SMALL 0.0001 +#define ORTHOG_PENALTY 1.0 + +float hmd_pos[NUM_HMD][3]; +void ReadHmdPoints() +{ + int i; + FILE *fin = fopen(INDIR "HMD_points.csv","r"); + if (fin==NULL) { + printf("ERROR: could not open HMD_points.csv for reading\n"); + exit(1); + } + + for (i=0; i TOO_SMALL) { + float scaleRot = stepSizeRot / gradSizeRot; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + T[j][k] += scaleRot * conj[j][k]; + } + } + stepSizeRot *= falloff; + } + + if (gradSizePos > TOO_SMALL) { + float scalePos = stepSizePos / gradSizePos; + for (j=0; j<3; j++) { + T[j][3] += scalePos * conj[j][3]; + } + stepSizePos *= falloff; + } + + // Constrain the gradient (such that scaling is one) + if (constrain) + { + // Measure the scales + float len[3] = {0.0, 0.0, 0.0}; + for (j=0; j<3; j++) { + double lenSq = 0.0; + for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } + len[j] = sqrt(lenSq); + } + + // How far off is the scale? + float xzLen = 0.5 * (len[0] + len[2]); + if (xzLen > TOO_SMALL) { + float inv_xzLen = 1.0 / xzLen; + for (j=0; j<3; j++) { + T[3][j] *= inv_xzLen; + } + } + + // Rescale the thing + for (j=0; j<3; j++) + { + if (len[j] > TOO_SMALL) { + float inv_len = 1.0 / len[j]; + for (k=0; k<3; k++) { T[j][k] *= inv_len; } + } + } + } + } + float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); + printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); +} + +int main() +{ + int i,j,k,sen,axis; + + // Read the data files + ReadHmdPoints(); + ReadPtinfo(); + + //------------------------- + // Package the lighthouse data for "AffineSolve" + //------------------------- + + // Data for the "iterative" affine solve formula + float Tcalc[4][4]; + float O[MAX_POINTS][4]; + float N[MAX_POINTS][3]; + float D[MAX_POINTS]; + int nPlanes = 0; + + for (sen=0; sen 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } + if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } + z_y = -z_y; + } + y_y = -y_y; + } + x_y = -x_y; + } + printf("bestErr %f\n", bestErr); +*/ + + //------------------------- + // A test version of the rescaling to the proper length + //------------------------- + FLOAT ydist2; + FLOAT bestBestErr = 9999.0; + FLOAT bestYdist = 0; + for (ydist2=ydist-0.1; ydist2 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } + z_y = -z_y; + } + y_y = -y_y; + } + x_y = -x_y; + } + printf("ydist2 %f bestErr %f\n",ydist2,bestErr); + + if (bestErr < bestBestErr) { + memcpy(x,x2,3*sizeof(FLOAT)); + memcpy(y,y2,3*sizeof(FLOAT)); + memcpy(z,z2,3*sizeof(FLOAT)); + bestBestErr = bestErr; + bestYdist = ydist2; + } + } + ydist = bestYdist; + +/* + for (i=0; i +#include +#include +#include +#include "dclapack.h" + +#define LH_ID 1 +#define NUM_HMD 32 + +#define MAX_POINTS 128 +//#define _ABS(a) ( (a)<=0 ? -(a) : (a) ) +#define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f ) +#define RANDF ( (float)rand() / (float)RAND_MAX ) +#define PI 3.14159265358979323846264 + +#define STEP_SIZE_ROT 1.0 +#define STEP_SIZE_POS 1.0 +#define FALLOFF 0.99999 +#define NITER 2000000 +#define TOO_SMALL 0.0001 +#define ORTHOG_PENALTY 1.0 + +float hmd_pos[NUM_HMD][3]; +void ReadHmdPoints() +{ + int i; + FILE *fin = fopen("HMD_points.csv","r"); + if (fin==NULL) { + printf("ERROR: could not open HMD_points.csv for reading\n"); + exit(1); + } + + for (i=0; i TOO_SMALL) { + float scaleRot = stepSizeRot / gradSizeRot; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + T[j][k] += scaleRot * conj[j][k]; + } + } + stepSizeRot *= falloff; + } + + if (gradSizePos > TOO_SMALL) { + float scalePos = stepSizePos / gradSizePos; + for (j=0; j<3; j++) { + T[j][3] += scalePos * conj[j][3]; + } + stepSizePos *= falloff; + } + + // Constrain the gradient (such that scaling is one) + if (constrain) + { + // Measure the scales + float len[3] = {0.0, 0.0, 0.0}; + for (j=0; j<3; j++) { + double lenSq = 0.0; + for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } + len[j] = sqrt(lenSq); + } + + // How far off is the scale? + float xzLen = 0.5 * (len[0] + len[2]); + if (xzLen > TOO_SMALL) { + float inv_xzLen = 1.0 / xzLen; + for (j=0; j<3; j++) { + T[3][j] *= inv_xzLen; + } + } + + // Rescale the thing + for (j=0; j<3; j++) + { + if (len[j] > TOO_SMALL) { + float inv_len = 1.0 / len[j]; + for (k=0; k<3; k++) { T[j][k] *= inv_len; } + } + } + } + } + float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); + printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); +} + +int main() +{ + int i,j,k,sen,axis; + + // Read the data files + printf( "...\n" ); + ReadHmdPoints(); + ReadPtinfo(); + + //------------------------- + // Package the lighthouse data for "AffineSolve" + //------------------------- + + // Data for the "iterative" affine solve formula + // float Tcalc[4][4]; + float O[MAX_POINTS][4]; + float N[MAX_POINTS][3]; + float D[MAX_POINTS]; + int nPlanes = 0; + + for (sen=0; sen // Standard Header For Most Programs +#include +#include +#include +#include "os_generic.h" +#include "linmath.h" +#include "fileutil.h" + +#ifdef __APPLE__ +#include // The GL Header File +#include // The GL Utility Toolkit (Glut) Header +#else +#include +#include +#endif +#ifdef __linux__ +#include +#endif + +#define RegisterDriver(a,b) +#include "poser_daveortho.c" + + +// Required to set up a window +#define WIDTH 1280 +#define HEIGHT 1280 +#define FULLSCREEN 0 +int keys[256]; // Regular keyboard keys +int sp_keys[256]; // Special keyboard keycodes (GLUT specific) + +#define LH_ID 0 +#define NUM_HMD 32 +#define INDIR "dave/full_test_triangle_on_floor/" +#define MAX_POINTS SENSORS_PER_OBJECT + +#define PI 3.1415926535897932386264 +#define MOVESPEED 1.0 +#define ROTSPEED 5.0 + +// View space +float posx=0.0f; +float posy=0.0f; +float posz=0.0f; +float rotx=0.0f; +float roty=0.0f; +float rotz=0.0f; + +// Data for the "fake" ortho solve formula +float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix +FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets +int nPoints=0; + +//-------------------------------------------------------------------- +// +//-------------------------------------------------------------------- + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ); + +void DrawCoordinateSystem( + float x, float y, float z, + float qx, float qy, float qz, float qr); + + +float hmd_pos[NUM_HMD][3]; +void ReadHmdPoints() +{ + int i; + FILE *fin = fopen(INDIR "HMD_points.csv","r"); + if (fin==NULL) { + printf("ERROR: could not open " INDIR "HMD_points.csv for reading\n"); + exit(1); + } + + for (i=0; i= read_frameno-6 && + read_hmdAngleViewed[sweepy][i] >= read_frameno-6 && + hmdAngles[sweepx][i]!=-9999.0 && hmdAngles[sweepy][i]!=-9999.0) + { + S_in[0][nPoints] = hmdAngles[sweepy][i]; + S_in[1][nPoints] = hmdAngles[sweepx][i]; + X_in[0][nPoints] = hmd_pos[i][0]; + X_in[1][nPoints] = hmd_pos[i][1]; + X_in[2][nPoints] = hmd_pos[i][2]; +printf("i %d S %f %f X %f %f %f frno %d %d currfr %d\n", + i, S_in[0][nPoints], S_in[1][nPoints], + X_in[0][nPoints], X_in[1][nPoints], X_in[2][nPoints], + read_hmdAngleViewed[sweepx][i], read_hmdAngleViewed[sweepy][i], read_frameno); + nPoints++; + } + } + + read_frameno++; + + //-------------------------------------------------- + // Run the "OrthoSolve" and then the "AffineSolve" + //-------------------------------------------------- + + // Run OrthoSolve + OrthoSolve( + Tortho, // OUTPUT: 4x4 transformation matrix + S_out, // OUTPUT: array of output screenspace points + S_in, // INPUT: array of screenspace points + X_in, // INPUT: array of offsets + nPoints); + printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]); + + + //------------------------ + // Draw the inputs + //------------------------ + glPointSize(3.0); + + // Draw the input points + glColor3f(1.0,0.5,0.5); + glBegin(GL_POINTS); + for (i=0; i +#include + +int main() +{ + float A[ORDER][ORDER]; + float Ainv[ORDER][ORDER]; + float Prod[ORDER][ORDER]; + + int i, j, n = 3; + srand(7779); + + for(i=0; i +#include + +#define PI 3.14159265358979323846264 + + +og_mutex_t read_mutex; +og_thread_t read_thread; +double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +int read_frameno=0; + +static FILE *fopen_orDie(const char *path, const char *flag) +{ + FILE *f = fopen(path, flag); + if (f == NULL) { + printf("ERROR could not oepn %s for %s\n", path, flag); + exit(1); + } + return f; +} + +static void SeekToken(FILE *f, const char *keyword) +{ + char token[4096]; + do { + fscanf(f, "%s", token); + } while( strcmp(token,keyword)!=0 && !feof(f) ); +} + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal) +{ + FILE *f = fopen_orDie(path,"r"); + SeekToken(f, "POS:"); + fscanf(f, "%f %f %f\n", x, y, z); + SeekToken(f, "QUAT:"); + fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk); + fclose (f); +} + +void LoadHmdProcessedDataAngles( + const char *path, + double angles[NUM_SWEEP][NUM_HMD]) +{ + int i,j; + char type[256]; + char sweep[256]; + int id; + int nSweep; + double ang; + double d1,d2,d3; // revisit these numbers later + + // Initialize all of the angles to -9999 + for (i=0; iNUM_HMD) { continue; } + + OGLockMutex (read_mutex); + read_hmdAngles[sweepId][id] = angle; + OGUnlockMutex(read_mutex); + read_hmdAngleViewed[sweepId][id] = read_frameno; + } + } +} + + diff --git a/attic/dave/fileutil.h b/attic/dave/fileutil.h new file mode 100644 index 0000000..e5da244 --- /dev/null +++ b/attic/dave/fileutil.h @@ -0,0 +1,35 @@ +#ifndef _fileutil_h_ +#define _fileutil_h_ + +#include +#include "os_generic.h" + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal); + + +// first 32 are hmd, next 24 wm0 next 24 wm1 +#define NUM_HMD 80 +#define NUM_SWEEP 4 +#define SWEEP_LX 0 +#define SWEEP_LY 1 +#define SWEEP_RX 2 +#define SWEEP_RY 3 +void LoadHmdProcessedDataAngles( + const char *path, + double angle[NUM_SWEEP][NUM_HMD]); + + +extern og_mutex_t read_mutex; +extern og_thread_t read_thread; +extern double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +extern int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +extern int read_frameno; +void *ThreadReadHmtAngles(void *junk); + + +#endif // __fileutil_h_ + + diff --git a/attic/dave/kalman_filter.c b/attic/dave/kalman_filter.c new file mode 100644 index 0000000..3d3406a --- /dev/null +++ b/attic/dave/kalman_filter.c @@ -0,0 +1,72 @@ +#include +#include +#include +#include "kalman_filter.h" + +void KalmanPredict( + KAL_VEC(xhat_k_km1), /* OUTPUT: (S) Predicted state at time 'k' */ + KAL_MAT(P_k_km1), /* OUTPUT: (S x S) Predicted covariance at time 'k' */ + KAL_MAT(P_km1_km1), /* INPUT: (S x S) Updated covariance from time 'k-1' */ + KAL_VEC(xhat_km1_km1), /* INPUT: (S) Updated state from time 'k-1' */ + KAL_MAT(F_k), /* INPUT: (S x S) State transition model */ + KAL_MAT(B_k), /* INPUT: (S x U) Control input model */ + KAL_VEC(u_k), /* INPUT: (U) Control vector */ + KAL_MAT(Q_k), /* INPUT: (S x S) Covariance of process noise */ + int S, /* INPUT: Number of dimensions in state vector */ + int U) /* INPUT: Size of control input vector */ +{ + KAL_MAT(F_k_tran); + KAL_MAT(F_k__P_km1_km1); + + // Predicted state: xhat_k_km1 = Fk * xhat_km1_km1 + Bk * uk + MUL(F_k, xhat_km1_km1, xhat_k_km1, S,S,1); + + // Predicted covar: P_k_km1 = Fk * P_km1_km1 * Fk' + Qk + MUL(F_k, P_km1_km1, F_k__P_km1_km1, S, S, S); + TRANSP(F_k, F_k_tran, S, S); + MULADD(F_k__P_km1_km1, F_k_tran, Q_k, P_k_km1, S, S, S); +} + +void KalmanUpdate( + KAL_VEC(xhat_k_k), /* (S) OUTPUT: Updated state at time 'k' */ + KAL_MAT(P_k_k), /* (S x S) OUTPUT: Updated covariance at time 'k' */ + KAL_VEC(xhat_k_km1), /* (S) INPUT: Predicted state at time 'k' */ + KAL_MAT(P_k_km1), /* (S x S) INPUT: Predicted covariance at time 'k' */ + KAL_VEC(z_k), /* (B) INPUT: Observation vector */ + KAL_MAT(H_k), /* (B x S) INPUT: Observational model */ + KAL_MAT(R_k), /* (S x S) INPUT: Covariance of observational noise */ + int B, /* INPUT: Number of observations in observation vector */ + int S) /* INPUT: Number of measurements in the state vector */ +{ + // UPDATE PHASE + // Measurement residual: yhat_k = zk - Hk * xhat_k_km1 + KAL_MAT(yhat_k); /* (B x 1) */ + GMULADD(H_k,xhat_k_km1,z_k,yhat_k,-1.0f,1.0f,B,S,1); + + // Residual covariance: S_k = H_k * P_k_km1 * H_k' + R_k + KAL_MAT(H_k_transp); /* (S x B) */ + KAL_MAT(P_k_km1__H_k_transp); /* (S x B) */ + KAL_MAT(S_k); /* (B x B) */ + TRANSP(H_k,H_k_transp,B,S); + MUL(P_k_km1,H_k_transp,P_k_km1__H_k_transp,S,S,B); + MULADD(H_k,P_k_km1__H_k_transp,R_k,S_k,B,S,B); + + // Optimal Kalman gain: K_k = P_k_km1 * H_k' * inv(S_k) + KAL_MAT(K_k); /* (S x B) */ + KAL_MAT(S_k_inv); /* (B x B) */ + INV(S_k,S_k_inv,B); + MUL(P_K_km1__H_k_transp,S_k_inv,K_k,S,B,B); + + // Updated state esti: xhat_k_k = xhat_k_km1 + K_k * yhat_k + MULADD(K_k,yhat_k,xhat_k_km1,S,B,1); + + // Updated covariance: P_k_k = (I - K_k * H_k) * P_k_km1 + KAL_MAT(Ident); /* (S x S) */ + KAL_MAT(I_minus_K_k_H_k); + IDENTITY(Ident,S); + GMULADD(K_k,H_k,Ident,I_minus_K_k_H_k,1.0,-1.0,S,B,S); + MUL(I_minus_K_k_H_k,P_k_km1,P_k_k,S,S,1); +} + + + diff --git a/attic/dave/kalman_filter.h b/attic/dave/kalman_filter.h new file mode 100644 index 0000000..6511fac --- /dev/null +++ b/attic/dave/kalman_filter.h @@ -0,0 +1,116 @@ +#ifndef __KALMAN_FILTER_H__ +#define __KALMAN_FILTER_H__ + +#include "dclapack.h" + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * Legend: + * xhat_k_km1 -- Predicted state at time 'k' using information available at time 'k-1' + * P_k_km1 -- Predicted covariance at time 'k' using information available at time 'k-1' + * F_k -- State transition model + * u_k -- Control vector (i.e. external process) + * B_k -- Control input model + * w_k -- Gaussian White noise + * H_k -- Observation model (to transform true state into measurements) + * Q_k -- Covariance matrix of process noise + * R_k -- Covariance of observational noise + * v_k -- Gaussian white observational noise + * + * PREDICTION PHASE + * Predicted state: xhat_k_km1 = Fk * xhat_km1_km1 + Bk * uk + * Predicted covar: P_k_km1 = Fk * P_km1_km1 * Fk' + Qk + * + * UPDATE PHASE + * Measurement residual: yhat_k = zk - Hk * xhat_k_km1 + * Residual covariance: S_k = H_k * P_k_km1 * H_k' + R_k + * Optimal Kalman gain: K_k = P_k_km1 * H_k' * inv(S_k) + * Updated state esti: xhat_k_k = xhat_k_km1 + K_k * yhat_k + * Updated covariance: P_k_k = (I - K_k * H_k) * P_k_km1 + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ + +#define KAL_STRIDE 36 +#define KAL_MAT(_var) float _var[ORDER][ORDER] +#define KAL_VEC(_var) float _var[ORDER][ORDER] + +void KalmanPredict( + KAL_VEC(xhat_k_km1), /* OUTPUT: (S) Predicted state at time 'k' */ + KAL_MAT(P_k_km1), /* OUTPUT: (S x S) Predicted covariance at time 'k' */ + KAL_MAT(P_km1_km1), /* INPUT: (S x S) Updated covariance from time 'k-1' */ + KAL_VEC(xhat_km1_km1), /* INPUT: (S) Updated state from time 'k-1' */ + KAL_MAT(F_k), /* INPUT: (S x S) State transition model */ + KAL_MAT(B_k), /* INPUT: (S x U) Control input model */ + KAL_VEC(u_k), /* INPUT: (U) Control vector */ + KAL_MAT(Q_k), /* INPUT: (S x S) Covariance of process noise */ + int S, /* INPUT: Number of dimensions in state vector */ + int U); /* INPUT: Size of control input vector */ + +void KalmanUpdate( + KAL_VEC(xhat_k_k), /* (S) OUTPUT: Updated state at time 'k' */ + KAL_MAT(P_k_k), /* (S x S) OUTPUT: Updated covariance at time 'k' */ + KAL_VEC(xhat_k_km1), /* (S) INPUT: Predicted state at time 'k' */ + KAL_MAT(P_k_km1), /* (S x S) INPUT: Predicted covariance at time 'k' */ + KAL_VEC(z_k), /* (B) INPUT: Observation vector */ + KAL_MAT(H_k), /* (B x S) INPUT: Observational model */ + KAL_MAT(R_k), /* (S x S) INPUT: Covariance of observational noise */ + int B, /* INPUT: Number of observations in observation vector */ + int S); /* INPUT: Number of measurements in the state vector */ + +/* * * * * * * * * * * * + * State vector format: + * xhat[12] = { x, y, z, ix, iy, iz, jx, jy, jz, kx, ky, kz, vx, vy, vz, vix, viy, viz, vjx, vjy, vjz, vkx, vky, vkz, ax, ay, az, aix, aiy, aiz, ajx, ajy, ajz, akx, aky, akz }, + * 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 + * where, + * i -- right direction vector + * j -- forward direction vector + * k -- up direction vector + * * * * * * * * * * * */ + +/* positional state */ +#define ST_X 0 +#define ST_Y 1 +#define ST_Z 2 +#define ST_IX 3 +#define ST_IY 4 +#define ST_IZ 5 +#define ST_JX 6 +#define ST_JY 7 +#define ST_JZ 8 +#define ST_KX 9 +#define ST_KY 10 +#define ST_KZ 11 + +/* velocity state */ +#define ST_VX 12 +#define ST_VY 13 +#define ST_VZ 14 +#define ST_VIX 15 +#define ST_VIY 16 +#define ST_VIZ 17 +#define ST_VJX 18 +#define ST_VJY 19 +#define ST_VJZ 20 +#define ST_VKX 21 +#define ST_VKY 22 +#define ST_VKZ 23 + +/* acceleration state */ +#define ST_AX 24 +#define ST_AY 25 +#define ST_AZ 26 +#define ST_AIX 27 +#define ST_AIY 28 +#define ST_AIZ 29 +#define ST_AJX 30 +#define ST_AJY 31 +#define ST_AJZ 32 +#define ST_AKX 33 +#define ST_AKY 34 +#define ST_AKZ 35 + + +/* * * * * * * * * * * * + * Measurement: + * * * * * * * * * * * */ + +#endif + diff --git a/attic/dave/main.c b/attic/dave/main.c new file mode 100644 index 0000000..ff187aa --- /dev/null +++ b/attic/dave/main.c @@ -0,0 +1,29 @@ +#include +#include "kalman_filter.h" + +int main() +{ + KAL_VEC(xhat_k_km1); /* OUTPUT: (S) Predicted state at time 'k' */ + KAL_MAT(P_k_km1); /* OUTPUT: (S x S) Predicted covariance at time 'k' */ + KAL_MAT(P_km1_km1); /* INPUT: (S x S) Updated covariance from time 'k-1' */ + KAL_VEC(xhat_km1_km1); /* INPUT: (S) Updated state from time 'k-1' */ + KAL_MAT(F_k); /* INPUT: (S x S) State transition model */ + KAL_MAT(B_k); /* INPUT: (S x U) Control input model */ + KAL_VEC(u_k); /* INPUT: (U) Control vector */ + KAL_MAT(Q_k); /* INPUT: (S x S) Covariance of process noise */ + + KalmanPredict( + xhat_k_km1, /* OUTPUT: (S) Predicted state at time 'k' */ + P_k_km1, /* OUTPUT: (S x S) Predicted covariance at time 'k' */ + P_km1_km1, /* INPUT: (S x S) Updated covariance from time 'k-1' */ + xhat_km1_km1, /* INPUT: (S) Updated state from time 'k-1' */ + F_k, /* INPUT: (S x S) State transition model */ + B_k, /* INPUT: (S x U) Control input model */ + u_k, /* INPUT: (U) Control vector */ + Q_k, /* INPUT: (S x S) Covariance of process noise */ + 36, /* INPUT: Number of dimensions in state vector */ + 36); /* INPUT: Size of control input vector */ + + return 0; +} + diff --git a/attic/dave/olddata/HMD_normals.csv b/attic/dave/olddata/HMD_normals.csv new file mode 100644 index 0000000..9abb886 --- /dev/null +++ b/attic/dave/olddata/HMD_normals.csv @@ -0,0 +1,32 @@ +0.656529 0.080037 0.750042 +1.000000 0.000000 0.000000 +0.951033 0.192296 -0.241987 +0.863341 0.261141 -0.431796 +0.562083 0.827080 -0.000702 +0.556742 0.818617 -0.141085 +0.127514 0.360969 0.923819 +0.197328 0.721208 0.664019 +0.197328 -0.720504 0.664783 +0.460200 0.003066 0.887810 +0.025263 -0.748333 0.662842 +0.556742 -0.818766 -0.140217 +0.562083 -0.827081 0.000175 +0.863341 -0.261598 -0.431519 +0.951034 -0.192552 -0.241783 +0.656529 -0.079242 0.750127 +-0.197328 0.721208 0.664019 +-0.127514 0.360969 0.923819 +-0.556742 0.818617 -0.141085 +-0.562083 0.827080 -0.000702 +-0.863341 0.261141 -0.431796 +-0.951033 0.192296 -0.241987 +-1.000000 0.000000 0.000000 +-0.656529 0.080037 0.750042 +-0.656529 -0.079242 0.750127 +-0.951034 -0.192552 -0.241783 +-0.863341 -0.261598 -0.431519 +-0.562083 -0.827081 0.000175 +-0.556742 -0.818766 -0.140217 +-0.025263 -0.748333 0.662842 +-0.460200 0.003066 0.887810 +-0.197328 -0.720504 0.664783 diff --git a/attic/dave/olddata/HMD_points.csv b/attic/dave/olddata/HMD_points.csv new file mode 100644 index 0000000..9c8f61d --- /dev/null +++ b/attic/dave/olddata/HMD_points.csv @@ -0,0 +1,32 @@ +0.085242 0.017104 0.046493 +0.093076 -0.000052 0.035027 +0.086979 0.016748 0.020598 +0.089932 0.029368 0.029518 +0.080100 0.045401 0.034918 +0.050949 0.052772 0.033339 +0.024403 0.019970 0.059476 +0.047578 0.033637 0.053722 +0.047777 -0.034022 0.053513 +0.057882 -0.000007 0.056592 +0.027400 -0.051728 0.046844 +0.051043 -0.052940 0.032966 +0.080512 -0.045486 0.034783 +0.090041 -0.029387 0.029598 +0.086948 -0.016462 0.020596 +0.085268 -0.017169 0.046332 +-0.047738 0.033671 0.053643 +-0.024283 0.020141 0.059533 +-0.050979 0.052769 0.033118 +-0.080175 0.045313 0.034850 +-0.090020 0.029306 0.029481 +-0.087090 0.016787 0.020453 +-0.093113 0.000154 0.034906 +-0.085269 0.017293 0.046345 +-0.085128 -0.017079 0.046558 +-0.086970 -0.016675 0.020667 +-0.089911 -0.029458 0.029795 +-0.080444 -0.045268 0.034661 +-0.051102 -0.052996 0.033321 +-0.027283 -0.051820 0.046794 +-0.057974 -0.000051 0.056586 +-0.047652 -0.033921 0.053604 diff --git a/attic/dave/olddata/ptinfo.csv b/attic/dave/olddata/ptinfo.csv new file mode 100644 index 0000000..2dbef65 --- /dev/null +++ b/attic/dave/olddata/ptinfo.csv @@ -0,0 +1,88 @@ +0 0 0 1024 0.054304 8.087056 7.006970 0.000005 0.000095 49.911564 +0 0 1 1024 -0.091073 7.633667 2.124995 0.000002 0.000032 28.309638 +4 0 0 1024 0.046903 7.663818 9.134277 0.000014 0.000274 87.982354 +4 0 1 1024 -0.086459 8.770345 2.726730 0.000011 0.000192 29.506709 +5 0 0 1024 0.047980 4.033020 8.087671 0.000017 0.000218 50.499310 +5 0 1 1024 -0.077373 7.318115 2.557700 0.000005 0.000082 24.065775 +6 0 0 1024 0.062184 9.521423 7.429031 0.000003 0.000035 38.676732 +6 0 1 1024 -0.073973 9.523905 1.995981 0.000002 0.000029 26.113657 +7 0 0 1024 0.056254 9.999064 7.497290 0.000003 0.000044 38.734072 +7 0 1 1024 -0.079104 9.753560 2.103774 0.000004 0.000051 33.619688 +8 1 0 1024 -0.006291 9.665955 1.987640 0.000002 0.000028 15.453922 +8 1 1 1024 -0.117330 10.177124 1.600708 0.000001 0.000018 11.038096 +9 0 0 1025 0.062143 8.118984 6.820550 0.000004 0.000055 40.521475 +9 0 1 1023 -0.085365 8.509694 1.896965 0.000003 0.000091 21719098.365862 +9 1 0 1024 0.000557 6.315043 1.998499 0.000010 0.000087 21.686142 +9 1 1 1024 -0.113388 7.437968 1.836344 0.000002 0.000035 18.467548 +10 1 0 1024 -0.009943 9.343363 1.995316 0.000002 0.000031 17.584078 +10 1 1 1024 -0.124325 9.785502 1.689753 0.000002 0.000038 11.692163 +11 1 1 1024 -0.118290 6.393616 2.064949 0.000007 0.000127 19.804200 +12 1 0 1024 -0.014712 6.424642 2.678035 0.000011 0.000116 31.494265 +12 1 1 1024 -0.109233 7.731120 2.279953 0.000007 0.000190 18.323990 +15 0 0 1024 0.060565 6.858704 7.184426 0.000008 0.000088 44.052520 +15 0 1 1024 -0.094258 6.385498 2.018832 0.000010 0.000091 29.446135 +15 1 0 1024 -0.006425 3.219747 2.549749 0.000022 0.000239 26.656846 +15 1 1 1024 -0.106455 6.234049 1.960842 0.000006 0.000076 13.899017 +17 0 0 1024 0.066623 8.772827 6.714717 0.000003 0.000034 33.014899 +17 0 1 1024 -0.060226 8.462301 2.015937 0.000002 0.000022 32.443223 +23 1 1 1024 -0.155699 4.259481 2.133325 0.000009 0.000119 14.051344 +24 1 0 1024 0.004956 2.748250 3.060253 0.000032 0.000273 24.108629 +24 1 1 1024 -0.156870 7.030355 1.539771 0.000003 0.000047 11.937263 +27 1 0 1024 -0.004080 5.997660 2.117311 0.000010 0.000096 25.358188 +27 1 1 1024 -0.157007 8.123576 1.820049 0.000003 0.000063 11.321934 +28 1 0 1024 -0.007485 3.633423 2.815398 0.000022 0.000202 29.154675 +28 1 1 1024 -0.148765 6.027649 1.957405 0.000006 0.000081 13.776313 +29 1 0 1024 -0.006364 9.275757 2.122339 0.000002 0.000026 16.821591 +29 1 1 1024 -0.140631 9.910583 1.611250 0.000002 0.000023 13.197373 +30 0 0 1024 0.072943 3.733805 6.264887 0.000006 0.000071 35.965622 +30 0 1 1024 -0.052506 4.387878 3.479865 0.000009 0.000093 44.349223 +30 1 0 1024 0.008406 5.021810 2.308727 0.000008 0.000085 15.880164 +30 1 1 1024 -0.147555 8.227193 1.637723 0.000002 0.000022 13.866484 +31 1 0 1024 0.000033 9.308573 1.886001 0.000002 0.000017 19.936102 +31 1 1 1024 -0.145700 9.870667 1.610662 0.000001 0.000017 14.105948 +32 1 0 1016 -0.101837 5.457534 2.442544 0.000008 0.000085 16.869970 +32 1 1 1018 -0.100882 6.643766 1.484100 0.000002 0.000027 17.016439 +33 1 0 1019 -0.107913 5.854473 2.030354 0.000008 0.000111 17.656553 +33 1 1 1018 -0.104750 6.514285 1.579995 0.000002 0.000026 18.046024 +34 1 0 1020 -0.108257 6.882782 1.880494 0.000003 0.000038 14.125138 +34 1 1 1018 -0.110099 6.917526 1.892384 0.000002 0.000016 13.833723 +35 1 0 1022 -0.121796 8.334801 1.807161 0.000002 0.000028 14.423969 +35 1 1 1018 -0.107517 7.105231 1.554912 0.000002 0.000022 15.379143 +36 1 0 1021 -0.111978 8.395752 1.902573 0.000002 0.000021 12.684395 +36 1 1 1018 -0.103989 7.755075 1.513899 0.000002 0.000017 15.735186 +37 1 0 1022 -0.119498 8.144121 2.128056 0.000001 0.000016 11.799663 +37 1 1 1018 -0.086472 6.760928 1.440899 0.000002 0.000032 16.451576 +38 1 0 1022 -0.126362 8.366418 1.890943 0.000002 0.000024 15.547028 +38 1 1 1018 -0.099977 7.628377 1.647596 0.000003 0.000027 13.428394 +39 1 0 1022 -0.128867 8.546498 1.952486 0.000002 0.000020 13.761684 +39 1 1 1018 -0.091108 8.340987 1.392215 0.000002 0.000030 15.791367 +41 1 0 858 -0.117675 8.663534 2.024164 0.000002 0.000016 13.786543 +41 1 1 852 -0.083109 7.402558 1.346682 0.000001 0.000017 14.923250 +42 1 0 1020 -0.110949 8.397712 2.080674 0.000002 0.000028 16.436445 +42 1 1 1017 -0.080808 7.596423 1.468899 0.000002 0.000024 14.864047 +44 0 0 1021 0.098696 8.919217 4.402947 0.000003 0.000022 29.326257 +44 0 1 1023 -0.134452 8.670434 2.040941 0.000002 0.000027 25.185552 +54 0 0 1021 0.095352 9.054522 4.727093 0.000004 0.000049 27.342838 +54 0 1 1023 -0.158791 8.836144 2.087028 0.000003 0.000027 22.937738 +68 0 0 912 -0.126928 4.094275 2.984245 0.000000 0.000005 31.298089 +68 0 1 916 0.529440 2.825646 2.494563 0.000008 0.000066 26.030924 +68 1 0 916 0.282512 3.705604 1.662663 0.000004 0.000042 16.517113 +68 1 1 916 -0.382934 2.543668 1.671447 0.000012 0.000142 16.902817 +70 1 0 919 0.273220 2.914037 1.975858 0.000007 0.000072 16.422591 +70 1 1 911 -0.380586 2.538534 2.661676 0.000018 0.000189 21.306309 +71 1 0 927 0.270648 4.495415 1.832304 0.000003 0.000023 15.326381 +71 1 1 911 -0.374629 3.509147 1.960478 0.000007 0.000067 14.023731 +72 0 0 922 -0.139030 4.265365 3.092647 0.000001 0.000008 36.803851 +72 0 1 937 0.507338 4.919313 2.656878 0.000005 0.000087 28.489847 +72 1 0 918 0.273990 4.384191 1.657099 0.000003 0.000025 16.250508 +72 1 1 901 -0.365903 4.259850 1.754957 0.000004 0.000068 12.246038 +73 0 0 909 -0.131142 4.841928 3.053060 0.000001 0.000013 35.932353 +73 0 1 939 0.512391 5.133209 2.178137 0.000004 0.000025 26.796286 +73 1 0 915 0.278769 2.556967 1.691021 0.000006 0.000050 15.318204 +74 0 0 919 -0.135500 5.774937 2.990444 0.000000 0.000004 38.680779 +74 0 1 919 0.517127 8.180404 2.131625 0.000001 0.000012 24.272923 +74 1 0 916 0.282418 1.150973 1.692552 0.000023 0.000215 15.138841 +76 0 0 916 -0.136047 5.623431 3.027347 0.000001 0.000015 40.599860 +76 0 1 913 0.537758 7.682777 2.005741 0.000001 0.000008 23.670972 +86 0 0 925 -0.141096 6.131959 3.056302 0.000001 0.000009 37.086914 +86 0 1 929 0.512093 8.391079 2.034693 0.000001 0.000008 26.888878 diff --git a/attic/dave/ptinfo.csv b/attic/dave/ptinfo.csv new file mode 100644 index 0000000..f4c5ea9 --- /dev/null +++ b/attic/dave/ptinfo.csv @@ -0,0 +1,69 @@ +0 0 0 1024 0.041750 3.356344 1.828998 0.000002 0.000021 49.839622 +0 0 1 1024 0.109967 4.153035 1.883659 0.000002 0.000017 18.750043 +0 1 1 838 -0.207362 0.514121 2.011177 0.000005 0.000146 20.057562 +4 1 0 1024 0.131400 2.764364 2.038156 0.000003 0.000066 21.627449 +4 1 1 1024 -0.201195 2.843648 1.844738 0.000003 0.000079 20.542771 +6 0 0 1024 0.023004 4.443075 1.777077 0.000001 0.000026 47.120957 +6 0 1 1024 0.113830 5.225403 1.653571 0.000001 0.000022 20.887187 +6 1 0 1024 0.146652 2.742981 2.012739 0.000001 0.000012 21.375524 +6 1 1 1024 -0.211721 2.358521 1.862361 0.000001 0.000015 20.053232 +7 1 0 1024 0.140333 3.198140 1.997439 0.000001 0.000018 18.166517 +7 1 1 1024 -0.208156 2.908244 1.892947 0.000001 0.000013 17.755360 +8 0 0 1024 0.036998 5.118429 1.829670 0.000001 0.000012 39.822422 +8 0 1 1024 0.126802 6.585714 1.734635 0.000002 0.000021 20.020662 +9 0 0 1024 0.035700 4.496134 1.846496 0.000001 0.000012 45.663926 +9 0 1 1024 0.115987 5.774577 1.682445 0.000002 0.000027 19.873340 +10 0 0 1024 0.032992 4.960815 1.840030 0.000002 0.000034 49.867023 +10 0 1 1024 0.134889 6.200765 1.801494 0.000004 0.000056 19.108826 +15 0 0 1024 0.046206 3.663981 1.957251 0.000001 0.000022 51.132112 +15 0 1 1024 0.119199 4.899963 1.761027 0.000002 0.000060 24.645105 +16 1 0 1024 0.167758 2.818949 2.055596 0.000001 0.000010 19.765362 +16 1 1 1024 -0.210253 2.650065 1.684485 0.000001 0.000010 20.046716 +17 0 0 1024 0.008123 4.042155 1.907654 0.000001 0.000016 44.942365 +17 0 1 1024 0.119126 5.346334 1.616305 0.000002 0.000038 23.873406 +17 1 0 1024 0.160551 2.538859 2.078290 0.000001 0.000011 18.503004 +17 1 1 1024 -0.212818 1.963074 1.653989 0.000001 0.000010 20.818517 +18 1 0 1024 0.169252 1.834696 2.095149 0.000003 0.000073 20.457114 +18 1 1 1024 -0.202781 1.857198 1.804126 0.000003 0.000083 25.433970 +19 1 0 1024 0.177362 2.038411 2.536636 0.000008 0.000194 22.828018 +19 1 1 1024 -0.204779 2.259420 2.005833 0.000009 0.000221 27.081790 +23 0 0 1025 -0.010332 3.968150 1.862948 0.000004 0.000068 45.777934 +23 0 1 1023 0.128696 6.093495 1.773672 0.000004 0.004550 19300251.477888 +24 0 0 993 -0.006030 4.369734 1.921640 0.000005 0.000129 48.222013 +24 0 1 992 0.138082 6.460706 1.786190 0.000005 0.000155 22.781613 +27 0 1 1024 0.147109 2.936829 2.604852 0.000022 0.000474 28.721504 +29 0 0 1024 0.016187 5.032145 1.921979 0.000003 0.000047 47.194243 +29 0 1 1024 0.140979 6.596395 1.815706 0.000005 0.000090 24.794637 +30 0 0 1024 0.000282 4.738342 1.756708 0.000001 0.000040 47.044401 +30 0 1 1024 0.128872 6.677734 1.748380 0.000002 0.000039 19.083389 +31 0 0 1024 0.007667 5.235433 1.898046 0.000001 0.000019 47.785930 +31 0 1 1024 0.137390 6.697835 1.762878 0.000002 0.000024 20.161993 +32 0 0 1019 -0.221131 4.109073 1.687081 0.000001 0.000008 39.847161 +32 0 1 1022 0.145009 4.683627 1.672801 0.000001 0.000004 18.169256 +33 0 0 1020 -0.217002 4.494730 1.552900 0.000001 0.000006 38.840936 +33 0 1 1021 0.150320 4.659607 1.476978 0.000001 0.000006 20.903208 +34 0 0 1019 -0.220660 4.394218 1.499063 0.000001 0.000007 36.572525 +34 0 1 1020 0.155998 4.846773 1.572007 0.000001 0.000008 20.786276 +35 0 0 1020 -0.207471 4.282680 1.416071 0.000000 0.000006 37.591605 +35 0 1 1020 0.158539 3.864747 1.512890 0.000001 0.000005 19.809991 +36 0 0 1020 -0.213648 1.838133 1.606175 0.000001 0.000014 42.311387 +36 0 1 1020 0.151255 1.933946 1.611633 0.000001 0.000010 20.868507 +36 1 0 1017 0.357442 1.409087 2.339893 0.000001 0.000009 25.252360 +37 0 0 1019 -0.199851 4.357601 1.549965 0.000001 0.000006 41.234504 +37 0 1 1022 0.139227 4.483753 1.584095 0.000001 0.000008 19.873120 +38 0 0 1019 -0.200025 2.547739 1.691084 0.000001 0.000006 40.679205 +38 0 1 1020 0.153593 3.013930 1.747689 0.000001 0.000006 18.072496 +39 0 1 1022 0.147554 2.405292 1.636333 0.000001 0.000009 16.334833 +40 0 1 1022 0.134857 1.219056 1.504621 0.000004 0.000039 18.954468 +40 1 0 1020 0.341554 2.105331 2.496244 0.000000 0.000007 25.552542 +40 1 1 1020 -0.141616 2.311785 2.012586 0.000001 0.000007 22.408698 +41 1 0 1020 0.344930 2.430515 2.448566 0.000000 0.000003 27.764761 +41 1 1 1021 -0.149021 2.758386 1.892542 0.000000 0.000006 25.399428 +42 0 0 1020 -0.205661 1.525102 1.704018 0.000002 0.000045 38.874329 +42 0 1 1022 0.131522 1.181813 1.789474 0.000003 0.000023 19.790870 +42 1 0 1018 0.349915 3.181974 2.424394 0.000000 0.000005 27.068381 +42 1 1 1021 -0.147299 2.959639 1.892314 0.000000 0.000004 23.454554 +54 1 0 1019 0.346022 2.557082 2.295552 0.000000 0.000006 25.170448 +54 1 1 1020 -0.141488 2.866462 2.025173 0.000001 0.000006 25.737504 +55 1 0 1018 0.350800 1.716110 2.326650 0.000001 0.000013 26.234609 +55 1 1 1021 -0.145191 0.853330 1.730182 0.000001 0.000021 26.866658 -- cgit v1.2.3