int wrapper_clapack_sgetri(const enum CBLAS_ORDER Order, const int N, float *A, const int lda, const int *ipiv)
{
return clapack_sgetri(Order, N, A, lda, ipiv);
}
void rovioKalmanFilter(kalmanFilter *kf, float *meas_S1, float *meas_S2, float *predicted) {
int i;
/* Variables for matrix inversion */
int mtrx_sz = FILTER_SIZE;
int info, lwork;
int *ipiv = NULL;
float *work = NULL;
// some temp variables that we need
float temp[FILTER_SIZE * FILTER_SIZE];
float temp2[FILTER_SIZE * FILTER_SIZE];
float temp3[FILTER_SIZE * FILTER_SIZE];
float Ptmp[FILTER_SIZE * FILTER_SIZE];
float eye[FILTER_SIZE * FILTER_SIZE];
float new_state[FILTER_SIZE];
/* Clear the temp matricies that aren't directly overwritten */
memset(eye, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
/* Allocate ipiv and work */
ipiv = (int *) malloc(mtrx_sz * sizeof(int));
lwork = FILTER_SIZE * FILTER_SIZE;
work = (float *)malloc(sizeof(float) * lwork);
if(!ipiv || !work) {
if(ipiv)
free(ipiv);
if(work)
free(work);
printf("Error allocating memory!\n");
return;
}
/**** 2. Propagate the Covariance Matrix ****/
/* temp2 = Phi * P */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
kf->Phi, FILTER_SIZE, kf->P, FILTER_SIZE, 0.0f, temp2, FILTER_SIZE);
/* temp = temp2 * Phi' */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
temp2, FILTER_SIZE, kf->Phi, FILTER_SIZE, 0.0f, temp, FILTER_SIZE);
/* P = temp + Q */
for(i=0; i<FILTER_SIZE*FILTER_SIZE; i++)
kf->P[i] = temp[i] + kf->Q[i];
/**** 3. Propagate the model track estimate ****/
/* new_state = Phi * current_state */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, 1, FILTER_SIZE, 1.0f,
kf->Phi, FILTER_SIZE, kf->current_state, 1, 0.0f, new_state, 1);
for(i=0; i<3; i++) {
kf->residual_s1[i] = meas_S1[i] - new_state[i];
kf->residual_s2[i] = meas_S2[i] - new_state[i];
}
for(i=3; i<9; i++) {
kf->residual_s1[i] = 0;
kf->residual_s2[i] = 0;
}
/* temp = P + R1 */
for(i=0; i<FILTER_SIZE * FILTER_SIZE; i++)
temp[i] = kf->P[i] + kf->R1[i];
/* Invert temp by first performing an LU Decomposition */
info = clapack_sgetrf(CblasRowMajor, mtrx_sz, mtrx_sz, temp, mtrx_sz, ipiv);
/* Now, invert given the LU decomposition */
info = clapack_sgetri(CblasRowMajor, mtrx_sz, temp, mtrx_sz, ipiv);
/* W1 = P * temp */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
kf->P, FILTER_SIZE, temp, FILTER_SIZE, 0.0f, kf->W1, FILTER_SIZE);
/* temp = P + R2 */
for(i=0; i<FILTER_SIZE * FILTER_SIZE; i++)
temp[i] = kf->P[i] + kf->R2[i];
/* Invert temp by first performing an LU Decomposition */
info = clapack_sgetrf(CblasRowMajor, mtrx_sz, mtrx_sz, temp, mtrx_sz, ipiv);
/* Now, invert given the LU decomposition */
info = clapack_sgetri(CblasRowMajor, mtrx_sz, temp, mtrx_sz, ipiv);
/* W2 = P * temp */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
kf->P, FILTER_SIZE, temp, FILTER_SIZE, 0.0f, kf->W2, FILTER_SIZE);
/**** 6. Update the estimate ****/
/* W1 * residual_s1' */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, FILTER_SIZE, 1, 1.0f,
kf->W1, FILTER_SIZE, kf->residual_s1, FILTER_SIZE, 0.0f, temp, 1);
/* W2 * residual_s2' */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, FILTER_SIZE, 1, 1.0f,
kf->W2, FILTER_SIZE, kf->residual_s2, FILTER_SIZE, 0.0f, temp2, 1);
/* temp = temp + temp2 */
for(i=0; i<FILTER_SIZE; i++)
temp[i] = temp[i] + temp2[i];
/* predicted = new_state + temp */
for(i=0; i<FILTER_SIZE; i++){
predicted[i] = new_state[i] + temp[i];
kf->current_state[i] = predicted[i];
}
/* Clear the temp vars */
memset(temp, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
memset(temp2, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
diag(eye, 1);
/* temp3 = eye - W1 */
for(i=0; i<FILTER_SIZE*FILTER_SIZE; i++)
temp3[i] = eye[i] - kf->W1[i];
/* temp2 = temp3 * P */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
temp3, FILTER_SIZE, kf->P, FILTER_SIZE, 0.0f, temp2, FILTER_SIZE);
/* temp = temp2 * (temp3)' */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
temp2, FILTER_SIZE, temp3, FILTER_SIZE, 0.0f, temp, FILTER_SIZE);
/* temp3 = W1 * R1 */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
kf->W1, FILTER_SIZE, kf->R1, FILTER_SIZE, 0.0f, temp3, FILTER_SIZE);
/* temp2 = temp2 * W1' */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
temp3, FILTER_SIZE, kf->W1, FILTER_SIZE, 0.0f, temp2, FILTER_SIZE);
/* Ptmp = temp + temp2 */
for(i=0; i<FILTER_SIZE*FILTER_SIZE; i++)
Ptmp[i] = temp[i] + temp2[i];
/* temp3 = eye - W2 */
for(i=0; i<FILTER_SIZE*FILTER_SIZE; i++)
temp3[i] = eye[i] - kf->W2[i];
/* temp2 = temp3 * P */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
temp3, FILTER_SIZE, kf->P, FILTER_SIZE, 0.0f, temp2, FILTER_SIZE);
/* temp = temp2 * (temp3)' */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
temp2, FILTER_SIZE, temp3, FILTER_SIZE, 0.0f, temp, FILTER_SIZE);
/* temp3 = W2 * R2 */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
kf->W2, FILTER_SIZE, kf->R2, FILTER_SIZE, 0.0f, temp3, FILTER_SIZE);
/* P = temp2 * W2' */
cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, FILTER_SIZE, FILTER_SIZE, FILTER_SIZE, 1.0f,
temp3, FILTER_SIZE, kf->W2, FILTER_SIZE, 0.0f, temp2, FILTER_SIZE);
/* P = Ptmp + temp + temp2 */
for(i=0; i<FILTER_SIZE*FILTER_SIZE; i++)
kf->P[i] = Ptmp[i] + temp[i] + temp2[i];
// pmat(P);
free(ipiv);
return;
}
