diff options
Diffstat (limited to 'dave')
-rw-r--r-- | dave/Makefile | 5 | ||||
-rw-r--r-- | dave/dclapack.h | 39 | ||||
-rwxr-xr-x | dave/dclapack_test | bin | 0 -> 12836 bytes | |||
-rw-r--r-- | dave/dclapack_test.c | 3 | ||||
-rw-r--r-- | dave/kalman_filter.c | 160 | ||||
-rw-r--r-- | dave/kalman_filter.h | 71 |
6 files changed, 108 insertions, 170 deletions
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<stdio.h> +#define _ABS(a) ( (a)<=0 ? 0-(a) : (a) ) + /* * Prints a matrix A (n by m) */ @@ -39,6 +41,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<n; i++) { \ + for (j=0; j<m; j++) { \ + B[j][i] = A[i][j]; \ + } \ + } \ +} + +/* * Calculate L,U of a matrix A with pivot table */ #define LU(A,L,U,Piv,n) { \ @@ -55,7 +71,7 @@ \ int max=i; \ for (j=i+1; j<n; j++) { \ - if (U[j][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<n; k++) { \ @@ -185,4 +201,25 @@ PRINT(Ainv,n,n); \ } \ } +/* + * Matrix Multiply D = alpha * A * B + beta * C + * A (n by m) + * B (m by p) + * C (n by p) + * D (n by p) + */ +#define GMULADD(A,B,C,D,alpha,beta,n,m,p) { \ + int i,j,k; \ + float sum; \ + for (i=0; i<n; i++) { \ + for (j=0; j<p; j++) { \ + sum = 0.0f; \ + for (k=0; k<m; k++) { \ + sum += A[i][k] * B[k][j]; \ + } \ + D[i][j] = alpha * sum + beta * C[i][j]; \ + } \ + } \ +} + #endif diff --git a/dave/dclapack_test b/dave/dclapack_test Binary files differnew file mode 100755 index 0000000..7789e78 --- /dev/null +++ b/dave/dclapack_test diff --git a/dave/dclapack_test.c b/dave/dclapack_test.c index 38ae651..b0667ef 100644 --- a/dave/dclapack_test.c +++ b/dave/dclapack_test.c @@ -11,7 +11,7 @@ int main() float Ainv[ORDER][ORDER]; float Prod[ORDER][ORDER]; - int i, j, n = 12; + int i, j, n = 3; srand(7779); for(i=0; i<n; i++) { @@ -24,6 +24,7 @@ int main() INV(A,Ainv,n); } + PRINT(A,n,n); PRINT(Ainv,n,n); MUL(A,Ainv,Prod,n,n,n); PRINT(Prod,n,n); diff --git a/dave/kalman_filter.c b/dave/kalman_filter.c index eb8ba53..8403665 100644 --- a/dave/kalman_filter.c +++ b/dave/kalman_filter.c @@ -3,45 +3,6 @@ #include <string.h> #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<S; x++) { - for (y=0; y<S; y++) { - P_k_km1[x][y] = Q_k[x][y]; - } - } - sgemm_( - "n", // INPUT: Do we transpose A? 'n' no transpose, 't' transpose - "t", // 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__times__P_km1_km1, // INPUT: (float) Column array 'A' (M by K) - &stride, // INPUT: Column stride for 'A' - F_k, // INPUT: (float) Column array 'B' (K by N) - &stride, // INPUT: Column stride for 'B' - &zerof, // INPUT: Scaling factor for 'C' - P_k_km1, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE - &stride); // INPUT: Column stride for 'C' + // 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 + } -void KalmanUpdate( - KAL_VEC(xhat_k_k), /* OUTPUT: Updated state at time 'k' */ - KAL_MAT(P_k_k), /* OUTPUT: Updated covariance at time 'k' */ - KAL_VEC(xhat_k_km1), /* INPUT: Predicted state at time 'k' */ - KAL_MAT(P_k_km1), /* INPUT: Predicted covariance at time 'k' */ - KAL_MAT(H_k), /* INPUT: Observational model */ - KAL_MAT(R_k), /* INPUT: Covariance of observational noise */ - int B, /* INPUT: Number of observations in observation vector */ - int S) /* INPUT: Number of measurements in the state vector */ -{ -} - diff --git a/dave/kalman_filter.h b/dave/kalman_filter.h index d9f090a..6511fac 100644 --- a/dave/kalman_filter.h +++ b/dave/kalman_filter.h @@ -1,6 +1,8 @@ #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' @@ -27,8 +29,8 @@ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ #define KAL_STRIDE 36 -#define KAL_MAT(_var) float _var[KAL_STRIDE][KAL_STRIDE] -#define KAL_VEC(_var) float _var[KAL_STRIDE] +#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' */ @@ -43,14 +45,15 @@ void KalmanPredict( int U); /* INPUT: Size of control input vector */ void KalmanUpdate( - KAL_VEC(xhat_k_k), /* OUTPUT: Updated state at time 'k' */ - KAL_MAT(P_k_k), /* OUTPUT: Updated covariance at time 'k' */ - KAL_VEC(xhat_k_km1), /* INPUT: Predicted state at time 'k' */ - KAL_MAT(P_k_km1), /* INPUT: Predicted covariance at time 'k' */ - KAL_MAT(H_k), /* INPUT: Observational model */ - KAL_MAT(R_k), /* INPUT: Covariance of observational noise */ - int B, /* INPUT: Number of observations in observation vector */ - int S); /* INPUT: Number of measurements in the state vector */ + 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: @@ -77,32 +80,32 @@ void KalmanUpdate( #define ST_KZ 11 /* velocity state */ -#define ST_VX 0 -#define ST_VY 1 -#define ST_VZ 2 -#define ST_VIX 3 -#define ST_VIY 4 -#define ST_VIZ 5 -#define ST_VJX 6 -#define ST_VJY 7 -#define ST_VJZ 8 -#define ST_VKX 9 -#define ST_VKY 10 -#define ST_VKZ 11 +#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 0 -#define ST_AY 1 -#define ST_AZ 2 -#define ST_AIX 3 -#define ST_AIY 4 -#define ST_AIZ 5 -#define ST_AJX 6 -#define ST_AJY 7 -#define ST_AJZ 8 -#define ST_AKX 9 -#define ST_AKY 10 -#define ST_AKZ 11 +#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 /* * * * * * * * * * * * |