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);*/
}
