From d1bb7ae144d499d9db554c51926e52aebb332228 Mon Sep 17 00:00:00 2001 From: ultramn Date: Sat, 17 Dec 2016 21:32:47 -0800 Subject: Fixed a bug with dclapack. --- dave/Makefile | 5 +- dave/dclapack.h | 39 ++++++++++++- dave/dclapack_test | Bin 0 -> 12836 bytes dave/dclapack_test.c | 3 +- dave/kalman_filter.c | 160 +++++++++------------------------------------------ dave/kalman_filter.h | 71 ++++++++++++----------- 6 files changed, 108 insertions(+), 170 deletions(-) create mode 100755 dave/dclapack_test (limited to 'dave') diff --git a/dave/Makefile b/dave/Makefile index a9e5c65..ed916f4 100644 --- a/dave/Makefile +++ b/dave/Makefile @@ -1,4 +1,5 @@ all: - gcc -o kalman_filter kalman_filter.c main.c -llapack -lblas -lm +# gcc -O3 -o kalman_filter kalman_filter.c main.c + gcc -O3 -o dclapack_test dclapack_test.c clean: - rm -f kalman_filter + rm -f kalman_filter dclapack_test diff --git a/dave/dclapack.h b/dave/dclapack.h index 36fad53..4e209d3 100644 --- a/dave/dclapack.h +++ b/dave/dclapack.h @@ -11,6 +11,8 @@ #include +#define _ABS(a) ( (a)<=0 ? 0-(a) : (a) ) + /* * Prints a matrix A (n by m) */ @@ -38,6 +40,20 @@ } \ } +/* + * B = Transpose(A) + * A is (n by m) + * B is (m by n) + */ +#define TRANSP(A,B,n,m) { \ + int i,j; \ + for (i=0; i U[max][i]) { max = j; } \ + if (_ABS(U[j][i]) > _ABS(U[max][i])) { max = j; } \ } \ _tempi=Piv[i]; Piv[i]=Piv[max]; Piv[max]=_tempi; \ for (k=i; k #include "kalman_filter.h" -/* - * Performs alpha * A * B + beta * C - */ -extern void sgemm_( - char *TRANSA, // INPUT: Do we transpose A? 'n' no transpose, 't' transpose - char *TRANSB, // INPUT: Do we transpose B? 'n' no transpose, 't' transpose - int *M, // INPUT: Size parameter 'M' - int *N, // INPUT: Size parameter 'N' - int *K, // INPUT: Size parameter 'K' - float *ALPHA, // INPUT: scaling coefficient for A * B - void *A, // INPUT: (float) Column array 'A' (M by K) - int *LDA, // INPUT: Column stride for 'A' - void *B, // INPUT: (float) Column array 'B' (K by N) - int *LDB, // INPUT: Column stride for 'B' - float *BETA, // INPUT: Scaling factor for 'C' - void *C, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE - int *LDC); // INPUT: Column stride for 'C' - -/* - * General A X = B solution - */ -extern void sgesv_( - int *N, // INPUT: The order of matrix A - int *NRHS, // INPUT: the number of columns of matrix B - void *A, // INPUT/OUTPUT: (float) Column array - // entry: (N by N) matrix A - // exit: (L and U) from factorization A = P*L*U - int *LDA, // INPUT: Column stride of A - int *IPIV, // OUTPUT: Ineger array (dimension N) the pivot indices of the permutation matrix - int *B, // INPUT/OUTPUT: (float) Column array - // entry: (N by NRHS) matrix of right hand side matrix 'B' - // exit: (if INFO=0) N-NRHS solution matrix 'X' - int *LDB, // INPUT: Column stride of B - int *INFO); // OUTPUT: Did it work? - // 0: success - // < 0: if INFO==-i, the -ith argument had illegal val - // > 0: if INFO== i, U(i,i) is exactly zero, thus the factorization is singular (error). - - 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' */ @@ -54,106 +15,41 @@ void KalmanPredict( int S, /* INPUT: Number of dimensions in state vector */ int U) /* INPUT: Size of control input vector */ { - /* Temporary storage */ - KAL_MAT(F_k__times__P_km1_km1); - #define B_k__times__u_k xhat_k_km1 /* This vector overwrite the same memory */ + KAL_MAT(F_k_tran); + KAL_MAT(F_k__P_km1_km1); - /* Fortran pass by pointer */ - float zerof=0.0f; - float onef=1.0f; - int one=1; - int stride=KAL_STRIDE; + // Predicted state: xhat_k_km1 = Fk * xhat_km1_km1 + Bk * uk + MUL(F_k, xhat_km1_km1, xhat_k_km1, S,S,1); - int x,y; - - /* Calculate B_k__times__u_k */ - if (U == 0) { - /* If no control input model, then zero out */ - memset(B_k__times__u_k, 0, S*sizeof(float)); - } else { + // 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); +} - /* Otherwise: B_k * u_k */ - sgemm_( - "n", // INPUT: Do we transpose A? 'n' no transpose, 't' transpose - "n", // INPUT: Do we transpose B? 'n' no transpose, 't' transpose - &S, // INPUT: Size parameter 'M' - &one, // INPUT: Size parameter 'N' - &U, // INPUT: Size parameter 'K' - &onef, // INPUT: scaling coefficient for A * B - B_k, // INPUT: (float) Column array 'A' (M by K) - &stride, // INPUT: Column stride for 'A' - u_k, // INPUT: (float) Column array 'B' (K by N) - &stride, // INPUT: Column stride for 'B' - &zerof, // INPUT: Scaling factor for 'C' - B_k__times__u_k, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE - &stride); // INPUT: Column stride for 'C' - } +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 + GMULADD(H_k,xhat_k_km1,z_k,D,-1.0f,1.0f,S,S,1); - /* Calculate xhat_k_km1 */ - sgemm_( - "n", // INPUT: Do we transpose A? 'n' no transpose, 't' transpose - "n", // INPUT: Do we transpose B? 'n' no transpose, 't' transpose - &S, // INPUT: Size parameter 'M' - &one, // INPUT: Size parameter 'N' - &S, // INPUT: Size parameter 'K' - &onef, // INPUT: scaling coefficient for A * B - F_k, // INPUT: (float) Column array 'A' (M by K) - &stride, // INPUT: Column stride for 'A' - xhat_km1_km1, // INPUT: (float) Column array 'B' (K by N) - &stride, // INPUT: Column stride for 'B' - &onef, // INPUT: Scaling factor for 'C' - xhat_k_km1, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE - &stride); // INPUT: Column stride for 'C' + // Residual covariance: S_k = H_k * P_k_km1 * H_k' + R_k - /* Calculate F_k * P_km1_km1*/ - sgemm_( - "n", // INPUT: Do we transpose A? 'n' no transpose, 't' transpose - "n", // INPUT: Do we transpose B? 'n' no transpose, 't' transpose - &S, // INPUT: Size parameter 'M' - &S, // INPUT: Size parameter 'N' - &S, // INPUT: Size parameter 'K' - &onef, // INPUT: scaling coefficient for A * B - F_k, // INPUT: (float) Column array 'A' (M by K) - &stride, // INPUT: Column stride for 'A' - P_km1_km1, // INPUT: (float) Column array 'B' (K by N) - &stride, // INPUT: Column stride for 'B' - &zerof, // INPUT: Scaling factor for 'C' - F_k__times__P_km1_km1, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE - &stride); // INPUT: Column stride for 'C' - /* Calculate P_k_km1 */ - for (x=0; x