aboutsummaryrefslogtreecommitdiff
path: root/dave
diff options
context:
space:
mode:
authorultramn <dchapm2@umbc.edu>2016-12-17 21:32:47 -0800
committerultramn <dchapm2@umbc.edu>2016-12-17 21:32:47 -0800
commitd1bb7ae144d499d9db554c51926e52aebb332228 (patch)
tree8a178412bb846f5f748d439e9a8dde090ae9c9d1 /dave
parentebb2d6b456dfddbf7edb6358d4b4c71bfdd85a55 (diff)
downloadlibsurvive-d1bb7ae144d499d9db554c51926e52aebb332228.tar.gz
libsurvive-d1bb7ae144d499d9db554c51926e52aebb332228.tar.bz2
Fixed a bug with dclapack.
Diffstat (limited to 'dave')
-rw-r--r--dave/Makefile5
-rw-r--r--dave/dclapack.h39
-rwxr-xr-xdave/dclapack_testbin0 -> 12836 bytes
-rw-r--r--dave/dclapack_test.c3
-rw-r--r--dave/kalman_filter.c160
-rw-r--r--dave/kalman_filter.h71
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
new file mode 100755
index 0000000..7789e78
--- /dev/null
+++ b/dave/dclapack_test
Binary files differ
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
/* * * * * * * * * * * *