void srcdkfTimeUpdate(srcdkf_t *f, float32_t *u, float32_t dt) { int S = f->S; // number of states int V = f->V; // number of noise variables int L; // number of sigma points float32_t *x = f->x.pData; // state estimate // float32_t *Sv = f->Sv.pData; // process covariance float32_t *Xa = f->Xa.pData; // augmented sigma points float32_t *xIn = f->xIn; // callback buffer float32_t *xOut = f->xOut; // callback buffer float32_t *xNoise = f->xNoise; // callback buffer float32_t *qrTempS = f->qrTempS.pData; int i, j; srcdkfCalcSigmaPoints(f, &f->Sv); L = f->L; // Xa = f(Xx, Xv, u, dt) for (i = 0; i < L; i++) { for (j = 0; j < S; j++) xIn[j] = Xa[j*L + i]; for (j = 0; j < V; j++) xNoise[j] = Xa[(S+j)*L + i]; f->timeUpdate(xIn, xNoise, xOut, u, dt); for (j = 0; j < S; j++) Xa[j*L + i] = xOut[j]; } // sum weighted resultant sigma points to create estimated state f->w0m = (f->hh - (float32_t)(S+V)) / f->hh; for (i = 0; i < S; i++) { int rOffset = i*L; x[i] = Xa[rOffset + 0] * f->w0m; for (j = 1; j < L; j++) x[i] += Xa[rOffset + j] * f->wim; } // update state covariance for (i = 0; i < S; i++) { int rOffset = i*(S+V)*2; for (j = 0; j < S+V; j++) { qrTempS[rOffset + j] = (Xa[i*L + j + 1] - Xa[i*L + S+V + j + 1]) * f->wic1; qrTempS[rOffset + S+V + j] = (Xa[i*L + j + 1] + Xa[i*L + S+V + j + 1] - 2.0*Xa[i*L + 0]) * f->wic2; } } //matrixDump("qrTempS", &f->qrTempS); //yield(1); qrDecompositionT_f32(&f->qrTempS, NULL, &f->SxT); // with transposition //matrixDump("SxT", &f->SxT); //yield(200); arm_mat_trans_f32(&f->SxT, &f->Sx); }
void srcdkfMeasurementUpdate(srcdkf_t *f, float32_t *u, float32_t *ym, int M, int N, float32_t *noise, SRCDKFMeasurementUpdate_t *measurementUpdate) { int S = f->S; // number of states float32_t *Xa = f->Xa.pData; // sigma points float32_t *xIn = f->xIn; // callback buffer float32_t *xNoise = f->xNoise; // callback buffer float32_t *xOut = f->xOut; // callback buffer float32_t *Y = f->Y.pData; // measurements from sigma points float32_t *y = f->y.pData; // measurement estimate float32_t *Sn = f->Sn.pData; // observation noise covariance float32_t *qrTempM = f->qrTempM.pData; float32_t *C1 = f->C1.pData; float32_t *C1T = f->C1T.pData; float32_t *C2 = f->C2.pData; float32_t *D = f->D.pData; float32_t *inov = f->inov.pData; // M x 1 matrix float32_t *xUpdate = f->xUpdate.pData; // S x 1 matrix float32_t *x = f->x.pData; // state estimate float32_t *Sx = f->Sx.pData; // float32_t *SxT = f->SxT.pData; // float32_t *Pxy = f->Pxy.pData; float32_t *Q = f->Q.pData; float32_t *qrFinal = f->qrFinal.pData; int L; // number of sigma points int i, j; // make measurement noise matrix if provided if (noise) { f->Sn.numRows = N; f->Sn.numCols = N; arm_fill_f32(0, f->Sn.pData, N*N); for (i = 0; i < N; i++) arm_sqrt_f32(fabsf(noise[i]), &Sn[i*N + i]); } // generate sigma points srcdkfCalcSigmaPoints(f, &f->Sn); L = f->L; // resize all N and M based storage as they can change each iteration f->y.numRows = M; f->Y.numRows = M; f->Y.numCols = L; f->qrTempM.numRows = M; f->qrTempM.numCols = (S+N)*2; f->Sy.numRows = M; f->Sy.numCols = M; f->SyT.numRows = M; f->SyT.numCols = M; f->SyC.numRows = M; f->SyC.numCols = M; f->Pxy.numCols = M; f->C1.numRows = M; f->C1T.numCols = M; f->C2.numRows = M; f->C2.numCols = N; f->D.numRows = M; f->D.numCols = S+N; f->K.numCols = M; f->inov.numRows = M; f->qrFinal.numCols = 2*S + 2*N; // Y = h(Xa, Xn) for (i = 0; i < L; i++) { for (j = 0; j < S; j++) xIn[j] = Xa[j*L + i]; for (j = 0; j < N; j++) xNoise[j] = Xa[(S+j)*L + i]; measurementUpdate(u, xIn, xNoise, xOut); for (j = 0; j < M; j++) Y[j*L + i] = xOut[j]; } // sum weighted resultant sigma points to create estimated measurement f->w0m = (f->hh - (float32_t)(S+N)) / f->hh; for (i = 0; i < M; i++) { int rOffset = i*L; y[i] = Y[rOffset + 0] * f->w0m; for (j = 1; j < L; j++) y[i] += Y[rOffset + j] * f->wim; } // calculate measurement covariance components for (i = 0; i < M; i++) { int rOffset = i*(S+N)*2; for (j = 0; j < S+N; j++) { float32_t c, d; c = (Y[i*L + j + 1] - Y[i*L + S+N + j + 1]) * f->wic1; d = (Y[i*L + j + 1] + Y[i*L + S+N + j + 1] - 2.0f*Y[i*L]) * f->wic2; qrTempM[rOffset + j] = c; qrTempM[rOffset + S+N + j] = d; // save fragments for future operations if (j < S) { C1[i*S + j] = c; C1T[j*M + i] = c; } else { C2[i*N + (j-S)] = c; } D[i*(S+N) + j] = d; } } //matrixDump("Y", &f->Y); //yield(1); //matrixDump("qrTempM", &f->qrTempM); qrDecompositionT_f32(&f->qrTempM, NULL, &f->SyT); // with transposition //matrixDump("SyT", &f->SyT); arm_mat_trans_f32(&f->SyT, &f->Sy); arm_mat_trans_f32(&f->SyT, &f->SyC); // make copy as later Div is destructive // create Pxy arm_mat_mult_f32(&f->Sx, &f->C1T, &f->Pxy); // K = (Pxy / SyT) / Sy matrixDiv_f32(&f->K, &f->Pxy, &f->SyT, &f->Q, &f->R, &f->AQ); matrixDiv_f32(&f->K, &f->K, &f->Sy, &f->Q, &f->R, &f->AQ); // x = x + k(ym - y) for (i = 0; i < M; i++) inov[i] = ym[i] - y[i]; /* if(M == 3) { printf("Measured :\t%8.4f\t%8.4f\t%8.4f\nCalculated:\t%8.4f\t%8.4f\t%8.4f\n", ym[0],ym[1],ym[2],y[0],y[1],y[2]); }*/ arm_mat_mult_f32(&f->K, &f->inov, &f->xUpdate); //matrixDump("K", &f->K); for (i = 0; i < S; i++) x[i] += xUpdate[i]; // build final QR matrix // rows = s // cols = s + n + s + n // use Q as temporary result storage f->Q.numRows = S; f->Q.numCols = S; arm_mat_mult_f32(&f->K, &f->C1, &f->Q); for (i = 0; i < S; i++) { int rOffset = i*(2*S + 2*N); for (j = 0; j < S; j++) qrFinal[rOffset + j] = Sx[i*S + j] - Q[i*S + j]; } f->Q.numRows = S; f->Q.numCols = N; arm_mat_mult_f32(&f->K, &f->C2, &f->Q); for (i = 0; i < S; i++) { int rOffset = i*(2*S + 2*N); for (j = 0; j < N; j++) qrFinal[rOffset + S+j] = Q[i*N + j]; } f->Q.numRows = S; f->Q.numCols = S+N; arm_mat_mult_f32(&f->K, &f->D, &f->Q); for (i = 0; i < S; i++) { int rOffset = i*(2*S + 2*N); for (j = 0; j < S+N; j++) qrFinal[rOffset + S+N+j] = Q[i*(S+N) + j]; } // Sx = qr([Sx-K*C1 K*C2 K*D]') // this method is not susceptable to numeric instability like the Cholesky is qrDecompositionT_f32(&f->qrFinal, NULL, &f->SxT); // with transposition arm_mat_trans_f32(&f->SxT, &f->Sx); }
void srcdkfTimeUpdateReduced(srcdkf_t *f, float32_t *u, float32_t dt, bool updateBias) { int S = f->S; // number of states int V; // number of noise variables int L; // number of sigma points float32_t *x = f->x.pData; // state estimate arm_matrix_instance_f32 *Sv; // process covariance float32_t *Xa = f->Xa.pData; // augmented sigma points float32_t *xIn = f->xIn; // callback buffer float32_t *xOut = f->xOut; // callback buffer float32_t *xNoise = f->xNoise; // callback buffer float32_t *qrTempS = f->qrTempS.pData; int i=0, j=0, k=0; V = f->V; Sv = &f->Sv; /* //if(!updateBias) { printf("II: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f 11:%8.4f 12:%8.4f 13:%8.4f 14:%8.4f\n", x[0],x[1],x[2],x[3],x[4], x[5],x[6],x[7],x[8],x[9], x[10],x[11],x[12],x[13]); usleep(100); //}*/ srcdkfCalcSigmaPoints(f, Sv); L = f->L; // Xa = f(Xx, Xv, u, dt) for (i = 0; i < L; i++) { // i is column // Create input values for (j = 0; j < S; j++) // j is row xIn[j] = Xa[j*L + i]; for (j = 0; j < V; j++) xNoise[j] = Xa[(S+j)*L + i]; // Do time update if(updateBias) { f->timeUpdate(xIn, xNoise, xOut, u, dt, updateBias); } else { if (i == 0) { // do time update and remember result f->timeUpdate(xIn, xNoise, xOut, u, dt, updateBias); for (k = 0; k < S; k++) { f->SxTemp.pData[k] = xOut[k]; } } else if (i < 1 + S + 9) { // this is a nonlinear problem do the calculations f->timeUpdate(xIn, xNoise, xOut, u, dt, updateBias); } else if (i < 1 + S + V) { // linear problem its not needed to do each calculation // because Sv is only trace matrix and medium is mean value for (k = 0; k < S; k++) { xOut[k] = f->SxTemp.pData[k]; } } else if (i < 1 + S + V + S + 9) { // this is a nonlinear problem do the calculations f->timeUpdate(xIn, xNoise, xOut, u, dt, updateBias); } else if (i < 1 + S + V + S + V) { // linear problem its not needed to do each calculation for (k = 0; k < S; k++) { xOut[k] = f->SxTemp.pData[k]; } } } // Save result to the augmented states for (j = 0; j < S; j++) { Xa[j*L + i] = xOut[j]; } /* if(startPosition > 0) { printf("xIn: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f 11:%8.4f 12:%8.4f 13:%8.4f 14:%8.4f\n", xIn[0],xIn[1],xIn[2],xIn[3],xIn[4], xIn[5],xIn[6],xIn[7],xIn[8],xIn[9], xIn[10],xIn[11],xIn[12],xIn[13]); usleep(200); } if(startPosition > 0) { printf("xNo: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f\n", xNoise[0],xNoise[1],xNoise[2],xNoise[3],xNoise[4], xNoise[5],xNoise[6],xNoise[7],xNoise[8],xNoise[9]); usleep(200); } if(startPosition > 0) { printf("xOu: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f 11:%8.4f 12:%8.4f 13:%8.4f 14:%8.4f\n", xOut[0],xOut[1],xOut[2],xOut[3],xOut[4], xOut[5],xOut[6],xOut[7],xOut[8],xOut[9], xOut[10],xOut[11],xOut[12],xOut[13]); usleep(200); }*/ } // sum weighted resultant sigma points to create estimated state f->w0m = (f->hh - (float32_t)(S+V)) / f->hh; for (i = 0; i < S; i++) { int rOffset = i*L; x[i] = Xa[rOffset + 0] * f->w0m; for (j = 1; j < L; j++) { x[i] += Xa[rOffset + j] * f->wim; } } /* //if(!updateBias) { printf("OO: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f 11:%8.4f 12:%8.4f 13:%8.4f 14:%8.4f\n", x[0],x[1],x[2],x[3],x[4], x[5],x[6],x[7],x[8],x[9], x[10],x[11],x[12],x[13]); usleep(200); //}*/ // update state covariance for (i = 0; i < S; i++) { int rOffset = i*(S+V)*2; for (j = 0; j < S+V; j++) { qrTempS[rOffset + j] = (Xa[i*L + j + 1] - Xa[i*L + S+V + j + 1]) * f->wic1; qrTempS[rOffset + S+V + j] = (Xa[i*L + j + 1] + Xa[i*L + S+V + j + 1] - 2.0f*Xa[i*L + 0]) * f->wic2; } } //matrixDump("qrTempS", &f->qrTempS); //yield(1); qrDecompositionT_f32(&f->qrTempS, NULL, &f->SxT); // with transposition //matrixDump("SxT", &f->SxT); //yield(200); arm_mat_trans_f32(&f->SxT, &f->Sx); /* usleep(1000000); printf("++++++++++++++++++++++++++++++++++++++++++++\n\n\n\n\n"); for(i = 0; i < f->Sx.numRows; i++) { for (j = 0; f->Sx.numCols; j++ ) { printf("%8.4f ",f->Sx.pData[i*f->Sx.numCols + j]); } printf("\n"); usleep(20000); } printf("++++++++++++++++++++++++++++++++++++++++++++\n\n\n\n\n"); usleep(1000000);*/ }