double update(double measurement)
{
measurementUpdate();
double result = X + (measurement - X) * K;
X = result;
return result;
}
void StateEstimator::run()
{
predictionUpdate();
if (number_new_measurements)
{
measurementUpdate();
number_new_measurements = 0;
}
}
void monteCarlo()
{
float sonar = SensorValue(sonar_sensor) - 6; // allow for distance between sonar and centre of wheelbase
//nxtDisplayCenteredTextLine(1, "sonar: %f", (float)sonar);
if (sonar < 10 || sonar > 150) {return;}
measurementUpdate(sonar);
normalisation();
resampling();
}
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);
}
double kalmanfilter_update(double measurement) {
measurementUpdate();
double result = X + (measurement - X) * K;
X = result;
return result;
}
