diff options
Diffstat (limited to 'dave/kalman_filter.c')
| -rw-r--r-- | dave/kalman_filter.c | 159 |
1 files changed, 159 insertions, 0 deletions
diff --git a/dave/kalman_filter.c b/dave/kalman_filter.c new file mode 100644 index 0000000..eb8ba53 --- /dev/null +++ b/dave/kalman_filter.c @@ -0,0 +1,159 @@ +#include <stdio.h> +#include <stdlib.h> +#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' */ + 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 */ +{ + /* 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 */ + + /* Fortran pass by pointer */ + float zerof=0.0f; + float onef=1.0f; + int one=1; + int stride=KAL_STRIDE; + + 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 { + + /* 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' + } + + /* 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' + + /* 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' +} + + +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 */ +{ +} + + |