diff options
Diffstat (limited to 'dave/kalman_filter.c')
| -rw-r--r-- | dave/kalman_filter.c | 160 |
1 files changed, 28 insertions, 132 deletions
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 */ -{ -} - |