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. --- dave/AffineSolve | Bin 40104 -> 0 bytes dave/AffineSolve.c | 783 --------------- dave/AffineSolve.c.CHARLES | 643 ------------- dave/HMD_normals.csv | 32 - dave/HMD_points.csv | 32 - dave/Makefile | 21 - dave/OrthoPlot.c | 451 --------- dave/dclapack_test | Bin 12960 -> 0 bytes dave/dclapack_test.c | 34 - dave/errors.txt | 2141 ------------------------------------------ dave/fileutil.c | 133 --- dave/fileutil.h | 35 - dave/kalman_filter.c | 72 -- dave/kalman_filter.h | 116 --- dave/main.c | 29 - dave/olddata/HMD_normals.csv | 32 - dave/olddata/HMD_points.csv | 32 - dave/olddata/ptinfo.csv | 88 -- dave/ptinfo.csv | 69 -- 19 files changed, 4743 deletions(-) delete mode 100755 dave/AffineSolve delete mode 100644 dave/AffineSolve.c delete mode 100644 dave/AffineSolve.c.CHARLES delete mode 100644 dave/HMD_normals.csv delete mode 100644 dave/HMD_points.csv delete mode 100644 dave/Makefile delete mode 100644 dave/OrthoPlot.c delete mode 100755 dave/dclapack_test delete mode 100644 dave/dclapack_test.c delete mode 100644 dave/errors.txt delete mode 100644 dave/fileutil.c delete mode 100644 dave/fileutil.h delete mode 100644 dave/kalman_filter.c delete mode 100644 dave/kalman_filter.h delete mode 100644 dave/main.c delete mode 100644 dave/olddata/HMD_normals.csv delete mode 100644 dave/olddata/HMD_points.csv delete mode 100644 dave/olddata/ptinfo.csv delete mode 100644 dave/ptinfo.csv (limited to 'dave') diff --git a/dave/AffineSolve b/dave/AffineSolve deleted file mode 100755 index bd93cbd..0000000 Binary files a/dave/AffineSolve and /dev/null differ diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c deleted file mode 100644 index 685062e..0000000 --- a/dave/AffineSolve.c +++ /dev/null @@ -1,783 +0,0 @@ -// -// 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/dave/fileutil.h b/dave/fileutil.h deleted file mode 100644 index e5da244..0000000 --- a/dave/fileutil.h +++ /dev/null @@ -1,35 +0,0 @@ -#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/dave/kalman_filter.c b/dave/kalman_filter.c deleted file mode 100644 index 3d3406a..0000000 --- a/dave/kalman_filter.c +++ /dev/null @@ -1,72 +0,0 @@ -#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/dave/kalman_filter.h b/dave/kalman_filter.h deleted file mode 100644 index 6511fac..0000000 --- a/dave/kalman_filter.h +++ /dev/null @@ -1,116 +0,0 @@ -#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/dave/main.c b/dave/main.c deleted file mode 100644 index ff187aa..0000000 --- a/dave/main.c +++ /dev/null @@ -1,29 +0,0 @@ -#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/dave/olddata/HMD_normals.csv b/dave/olddata/HMD_normals.csv deleted file mode 100644 index 9abb886..0000000 --- a/dave/olddata/HMD_normals.csv +++ /dev/null @@ -1,32 +0,0 @@ -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/dave/olddata/HMD_points.csv b/dave/olddata/HMD_points.csv deleted file mode 100644 index 9c8f61d..0000000 --- a/dave/olddata/HMD_points.csv +++ /dev/null @@ -1,32 +0,0 @@ -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/dave/olddata/ptinfo.csv b/dave/olddata/ptinfo.csv deleted file mode 100644 index 2dbef65..0000000 --- a/dave/olddata/ptinfo.csv +++ /dev/null @@ -1,88 +0,0 @@ -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/dave/ptinfo.csv b/dave/ptinfo.csv deleted file mode 100644 index f4c5ea9..0000000 --- a/dave/ptinfo.csv +++ /dev/null @@ -1,69 +0,0 @@ -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