static void UKF_GenerateSigmaPoints(UKF_Filter* ukf)
{
int cols = 0;
//state
float32_t *X = ukf->X_f32;
float32_t *tmpX = ukf->tmpX_f32;
//need to tune!!!
//covariance
//c*chol(P)'
arm_mat_chol_f32(&ukf->P);
arm_mat_scale_f32(&ukf->P, ukf->gamma, &ukf->P);
//////////////////////////////////////////////////////////////////////////
//generate sigma points
arm_mat_setcolumn_f32(&ukf->XSP, X, cols);
for(cols = 1; cols <= UKF_STATE_DIM; cols++){
arm_mat_getcolumn_f32(&ukf->P, tmpX, cols - 1);
arm_mat_add_f32(&ukf->X, &ukf->tmpX, &ukf->tmpX);
arm_mat_setcolumn_f32(&ukf->XSP, tmpX, cols);
}
for(cols = UKF_STATE_DIM + 1; cols < UKF_SP_POINTS; cols++){
arm_mat_getcolumn_f32(&ukf->P, tmpX, cols - 8/*UKF_STATE_DIM + 1*/);
arm_mat_sub_f32(&ukf->X, &ukf->tmpX, &ukf->tmpX);
arm_mat_setcolumn_f32(&ukf->XSP, tmpX, cols);
}
}
void UKF_Update(UKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt)
{
int col, row;
float32_t norm;
#ifndef USE_4TH_RUNGE_KUTTA
float32_t halfdx, halfdy, halfdz;
float32_t halfdt = 0.5f * dt;
#endif
//////////////////////////////////////////////////////////////////////////
float32_t q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
//
float32_t hx, hy, hz;
float32_t bx, bz;
float32_t _2mx, _2my, _2mz;
//
float32_t *X = ukf->X_f32, *Y = ukf->Y_f32;
float32_t *tmpX = ukf->tmpX_f32, *tmpY = ukf->tmpY_f32;
float32_t tmpQ[4];
//////////////////////////////////////////////////////////////////////////
//calculate sigma points
UKF_GenerateSigmaPoints(ukf);
//time update
//unscented transformation of process
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, 0);
#ifdef USE_4TH_RUNGE_KUTTA
tmpQ[0] = 0;
tmpQ[1] = tmpX[4];
tmpQ[2] = tmpX[5];
tmpQ[3] = tmpX[6];
Quaternion_RungeKutta4(tmpX, tmpQ, dt, 1);
#else
//
halfdx = halfdt * tmpX[4];
halfdy = halfdt * tmpX[5];
halfdz = halfdt * tmpX[6];
//
tmpQ[0] = tmpX[0];
tmpQ[1] = tmpX[1];
tmpQ[2] = tmpX[2];
tmpQ[3] = tmpX[3];
//model prediction
//simple way, pay attention!!!
//according to the actual gyroscope output
//and coordinate system definition
tmpX[0] = tmpQ[0] + (halfdx * tmpQ[1] + halfdy * tmpQ[2] + halfdz * tmpQ[3]);
tmpX[1] = tmpQ[1] - (halfdx * tmpQ[0] + halfdy * tmpQ[3] - halfdz * tmpQ[2]);
tmpX[2] = tmpQ[2] + (halfdx * tmpQ[3] - halfdy * tmpQ[0] - halfdz * tmpQ[1]);
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//Re-normalize Quaternion
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
tmpX[3] *= norm;
#endif
//
arm_mat_setcolumn_f32(&ukf->XSP, tmpX, 0);
for(row = 0; row < UKF_STATE_DIM; row++){
X[row] = ukf->Wm0 * tmpX[row];
}
for(col = 1; col < UKF_SP_POINTS; col++){
//
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, col);
//
#ifdef USE_4TH_RUNGE_KUTTA
tmpQ[0] = 0;
tmpQ[1] = tmpX[4];
tmpQ[2] = tmpX[5];
tmpQ[3] = tmpX[6];
Quaternion_RungeKutta4(tmpX, tmpQ, dt, 1);
#else
halfdx = halfdt * tmpX[4];
halfdy = halfdt * tmpX[5];
halfdz = halfdt * tmpX[6];
//
tmpQ[0] = tmpX[0];
tmpQ[1] = tmpX[1];
tmpQ[2] = tmpX[2];
tmpQ[3] = tmpX[3];
//model prediction
//simple way, pay attention!!!
//according to the actual gyroscope output
//and coordinate system definition
tmpX[0] = tmpQ[0] + (halfdx * tmpQ[1] + halfdy * tmpQ[2] + halfdz * tmpQ[3]);
tmpX[1] = tmpQ[1] - (halfdx * tmpQ[0] + halfdy * tmpQ[3] - halfdz * tmpQ[2]);
tmpX[2] = tmpQ[2] + (halfdx * tmpQ[3] - halfdy * tmpQ[0] - halfdz * tmpQ[1]);
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//re-normalize quaternion
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
tmpX[3] *= norm;
#endif
//
arm_mat_setcolumn_f32(&ukf->XSP, tmpX, col);
for(row = 0; row < UKF_STATE_DIM; row++){
X[row] += ukf->Wmi * tmpX[row];
}
}
for(col = 0; col < UKF_SP_POINTS; col++){
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, col);
arm_mat_sub_f32(&ukf->tmpX, &ukf->X, &ukf->tmpX);
arm_mat_setcolumn_f32(&ukf->tmpXSP, tmpX, col);
}
arm_mat_trans_f32(&ukf->tmpXSP, &ukf->tmpXSPT);
arm_mat_mult_f32(&ukf->tmpXSP, &ukf->Wc, &ukf->tmpWcXSP);
arm_mat_mult_f32(&ukf->tmpWcXSP, &ukf->tmpXSPT, &ukf->PX);
arm_mat_add_f32(&ukf->PX, &ukf->Q, &ukf->PX);
//////////////////////////////////////////////////////////////////////////
//recalculate sigma points
//UKF_GenerateSigmaPoints(ukf);
//measurement update
//unscented transformation of measurments
//////////////////////////////////////////////////////////////////////////
//Normalize accel and mag
norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
accel[0] *= norm;
accel[1] *= norm;
accel[2] *= norm;
//////////////////////////////////////////////////////////////////////////
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm;
mag[1] *= norm;
mag[2] *= norm;
_2mx = 2.0f * mag[0];
_2my = 2.0f * mag[1];
_2mz = 2.0f * mag[2];
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, 0);
//Auxiliary variables to avoid repeated arithmetic
//
q0q0 = tmpX[0] * tmpX[0];
q0q1 = tmpX[0] * tmpX[1];
q0q2 = tmpX[0] * tmpX[2];
q0q3 = tmpX[0] * tmpX[3];
q1q1 = tmpX[1] * tmpX[1];
q1q2 = tmpX[1] * tmpX[2];
q1q3 = tmpX[1] * tmpX[3];
q2q2 = tmpX[2] * tmpX[2];
q2q3 = tmpX[2] * tmpX[3];
q3q3 = tmpX[3] * tmpX[3];
//Reference direction of Earth's magnetic field
hx = _2mx * (0.5f - q2q2 - q3q3) + _2my * (q1q2 - q0q3) + _2mz *(q1q3 + q0q2);
hy = _2mx * (q1q2 + q0q3) + _2my * (0.5f - q1q1 - q3q3) + _2mz * (q2q3 - q0q1);
hz = _2mx * (q1q3 - q0q2) + _2my * (q2q3 + q0q1) + _2mz *(0.5f - q1q1 - q2q2);
arm_sqrt_f32(hx * hx + hy * hy, &bx);
bz = hz;
tmpY[0] = tmpX[0];
tmpY[1] = tmpX[1];
tmpY[2] = tmpX[2];
tmpY[3] = tmpX[3];
tmpY[4] = 2.0f * (q1q3 - q0q2);
tmpY[5] = 2.0f * (q2q3 + q0q1);
tmpY[6] = -1.0f + 2.0f * (q0q0 + q3q3);
tmpY[7] = tmpX[4];
tmpY[8] = tmpX[5];
tmpY[9] = tmpX[6];
tmpY[10] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2));
tmpY[11] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1));
tmpY[12] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2));
arm_mat_setcolumn_f32(&ukf->YSP, tmpY, 0);
for(row = 0; row < UKF_MEASUREMENT_DIM; row++){
Y[row] = ukf->Wm0 * tmpY[row];
}
for(col = 1; col < UKF_SP_POINTS; col++){
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, col);
//Auxiliary variables to avoid repeated arithmetic
//
q0q0 = tmpX[0] * tmpX[0];
q0q1 = tmpX[0] * tmpX[1];
q0q2 = tmpX[0] * tmpX[2];
q0q3 = tmpX[0] * tmpX[3];
q1q1 = tmpX[1] * tmpX[1];
q1q2 = tmpX[1] * tmpX[2];
q1q3 = tmpX[1] * tmpX[3];
q2q2 = tmpX[2] * tmpX[2];
q2q3 = tmpX[2] * tmpX[3];
q3q3 = tmpX[3] * tmpX[3];
//Reference direction of Earth's magnetic field
hx = _2mx * (0.5f - q2q2 - q3q3) + _2my * (q1q2 - q0q3) + _2mz *(q1q3 + q0q2);
hy = _2mx * (q1q2 + q0q3) + _2my * (0.5f - q1q1 - q3q3) + _2mz * (q2q3 - q0q1);
hz = _2mx * (q1q3 - q0q2) + _2my * (q2q3 + q0q1) + _2mz *(0.5f - q1q1 - q2q2);
arm_sqrt_f32(hx * hx + hy * hy, &bx);
bz = hz;
tmpY[0] = tmpX[0];
tmpY[1] = tmpX[1];
tmpY[2] = tmpX[2];
tmpY[3] = tmpX[3];
tmpY[4] = 2.0f * (q1q3 - q0q2);
tmpY[5] = 2.0f * (q2q3 + q0q1);
tmpY[6] = -1.0f + 2.0f * (q0q0 + q3q3);
tmpY[7] = tmpX[4];
tmpY[8] = tmpX[5];
tmpY[9] = tmpX[6];
tmpY[10] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2));
tmpY[11] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1));
tmpY[12] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2));
arm_mat_setcolumn_f32(&ukf->YSP, tmpY, col);
for(row = 0; row < UKF_MEASUREMENT_DIM; row++){
Y[row] += ukf->Wmi * tmpY[row];
}
}
for(col = 0; col < UKF_SP_POINTS; col++){
arm_mat_getcolumn_f32(&ukf->YSP, tmpY, col);
arm_mat_sub_f32(&ukf->tmpY, &ukf->Y, &ukf->tmpY);
arm_mat_setcolumn_f32(&ukf->tmpYSP, tmpY, col);
}
arm_mat_trans_f32(&ukf->tmpYSP, &ukf->tmpYSPT);
arm_mat_mult_f32(&ukf->tmpYSP, &ukf->Wc, &ukf->tmpWcYSP);
arm_mat_mult_f32(&ukf->tmpWcYSP, &ukf->tmpYSPT, &ukf->PY);
arm_mat_add_f32(&ukf->PY, &ukf->R, &ukf->PY);
//transformed cross-covariance
arm_mat_mult_f32(&ukf->tmpWcXSP, &ukf->tmpYSPT, &ukf->PXY);
//calculate kalman gate
//K = PXY * inv(PY);
arm_mat_inverse_f32(&ukf->PY, &ukf->tmpPY);
arm_mat_mult_f32(&ukf->PXY, &ukf->tmpPY, &ukf->K);
//state update
//X = X + K*(z - Y);
Y[0] = q[0] - Y[0];
Y[1] = q[1] - Y[1];
Y[2] = q[2] - Y[2];
Y[3] = q[3] - Y[3];
//
Y[4] = accel[0] - Y[4];
Y[5] = accel[1] - Y[5];
Y[6] = accel[2] - Y[6];
Y[7] = gyro[0] - Y[7];
Y[8] = gyro[1] - Y[8];
Y[9] = gyro[2] - Y[9];
//////////////////////////////////////////////////////////////////////////
Y[10] = mag[0] - Y[10];
Y[11] = mag[1] - Y[11];
Y[12] = mag[2] - Y[12];
arm_mat_mult_f32(&ukf->K, &ukf->Y, &ukf->tmpX);
arm_mat_add_f32(&ukf->X, &ukf->tmpX, &ukf->X);
//normalize quaternion
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
//covariance update
#if 0
//the default tuning parameters don't work properly
//so that use the simple update method below
//original update method
//P = PX - K * PY * K'
arm_mat_trans_f32(&ukf->K, &ukf->KT);
arm_mat_mult_f32(&ukf->K, &ukf->PY, &ukf->PXY);
arm_mat_mult_f32(&ukf->PXY, &ukf->KT, &ukf->P);
arm_mat_sub_f32(&ukf->PX, &ukf->P, &ukf->P);
#else
//must tune the P,Q,R
//simple update method
//P = PX - K * PXY'
arm_mat_trans_f32(&ukf->PXY, &ukf->PXYT);
arm_mat_mult_f32(&ukf->K, &ukf->PXYT, &ukf->P);
arm_mat_sub_f32(&ukf->PX, &ukf->P, &ukf->P);
#endif
}
void SRCKF_Update(SRCKF_Filter* srckf, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt)
{
//////////////////////////////////////////////////////////////////////////
float32_t q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
//
float32_t hx, hy, hz;
float32_t bx, bz;
float32_t _2mx, _2my, _2mz;
//
int col;
float32_t dQ[4];
float32_t norm;
float32_t *X = srckf->X_f32, *Y = srckf->Y_f32;
float32_t *tmpX = srckf->tmpX_f32, *tmpY = srckf->tmpY_f32;
float32_t *iKesi = srckf->iKesi_f32;
dQ[0] = 0.0f;
dQ[1] = (gyro[0] - X[4]);
dQ[2] = (gyro[1] - X[5]);
dQ[3] = (gyro[2] - X[6]);
//////////////////////////////////////////////////////////////////////////
//time update
for(col = 0; col < SRCKF_CP_POINTS; col++){
//evaluate the cubature points
arm_mat_getcolumn_f32(&srckf->Kesi, iKesi, col);
arm_mat_mult_f32(&srckf->S, &srckf->iKesi, &srckf->tmpX);
arm_mat_add_f32(&srckf->tmpX, &srckf->X, &srckf->tmpX);
arm_mat_setcolumn_f32(&srckf->XCP, tmpX, col);
//evaluate the propagated cubature points
Quaternion_RungeKutta4(tmpX, dQ, dt, 1);
arm_mat_setcolumn_f32(&srckf->XPCP, tmpX, col);
}
//estimate the predicted state
arm_mat_cumsum_f32(&srckf->XPCP, tmpX, X);
arm_mat_scale_f32(&srckf->X, srckf->W, &srckf->X);
//estimate the square-root factor of the predicted error covariance
for(col = 0; col < SRCKF_CP_POINTS; col++){
arm_mat_getcolumn_f32(&srckf->XPCP, tmpX, col);
arm_mat_sub_f32(&srckf->tmpX, &srckf->X, &srckf->tmpX);
arm_mat_scale_f32(&srckf->tmpX, srckf->SW, &srckf->tmpX);
arm_mat_setcolumn_f32(&srckf->XCM, tmpX, col);
}
//XCM[XPCP, SQ], SQ fill already
arm_mat_qr_decompositionT_f32(&srckf->XCM, &srckf->ST);
arm_mat_trans_f32(&srckf->ST, &srckf->S);
//////////////////////////////////////////////////////////////////////////
//measurement update
//normalize accel and magnetic
norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
accel[0] *= norm;
accel[1] *= norm;
accel[2] *= norm;
//////////////////////////////////////////////////////////////////////////
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm;
mag[1] *= norm;
mag[2] *= norm;
_2mx = 2.0f * mag[0];
_2my = 2.0f * mag[1];
_2mz = 2.0f * mag[2];
for(col = 0; col < SRCKF_CP_POINTS; col++){
//evaluate the cubature points
arm_mat_getcolumn_f32(&srckf->Kesi, iKesi, col);
arm_mat_mult_f32(&srckf->S, &srckf->iKesi, &srckf->tmpX);
arm_mat_add_f32(&srckf->tmpX, &srckf->X, &srckf->tmpX);
arm_mat_setcolumn_f32(&srckf->XCP, tmpX, col);
//evaluate the propagated cubature points
//auxiliary variables to avoid repeated arithmetic
q0q0 = tmpX[0] * tmpX[0];
q0q1 = tmpX[0] * tmpX[1];
q0q2 = tmpX[0] * tmpX[2];
q0q3 = tmpX[0] * tmpX[3];
q1q1 = tmpX[1] * tmpX[1];
q1q2 = tmpX[1] * tmpX[2];
q1q3 = tmpX[1] * tmpX[3];
q2q2 = tmpX[2] * tmpX[2];
q2q3 = tmpX[2] * tmpX[3];
q3q3 = tmpX[3] * tmpX[3];
//reference direction of earth's magnetic field
hx = _2mx * (0.5f - q2q2 - q3q3) + _2my * (q1q2 - q0q3) + _2mz *(q1q3 + q0q2);
hy = _2mx * (q1q2 + q0q3) + _2my * (0.5f - q1q1 - q3q3) + _2mz * (q2q3 - q0q1);
hz = _2mx * (q1q3 - q0q2) + _2my * (q2q3 + q0q1) + _2mz *(0.5f - q1q1 - q2q2);
arm_sqrt_f32(hx * hx + hy * hy, &bx);
bz = hz;
tmpY[0] = 2.0f * (q1q3 - q0q2);
tmpY[1] = 2.0f * (q2q3 + q0q1);
tmpY[2] = -1.0f + 2.0f * (q0q0 + q3q3);
tmpY[3] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2));
tmpY[4] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1));
tmpY[5] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2));
arm_mat_setcolumn_f32(&srckf->YPCP, tmpY, col);
}
//estimate the predicted measurement
arm_mat_cumsum_f32(&srckf->YPCP, tmpY, Y);
arm_mat_scale_f32(&srckf->Y, srckf->W, &srckf->Y);
//estimate the square-root of the innovation covariance matrix
for(col = 0; col < SRCKF_CP_POINTS; col++){
arm_mat_getcolumn_f32(&srckf->YPCP, tmpY, col);
arm_mat_sub_f32(&srckf->tmpY, &srckf->Y, &srckf->tmpY);
arm_mat_scale_f32(&srckf->tmpY, srckf->SW, &srckf->tmpY);
arm_mat_setcolumn_f32(&srckf->YCPCM, tmpY, col);
arm_mat_setcolumn_f32(&srckf->YCM, tmpY, col);
}
//YCM[YPCP, SR], SR fill already
arm_mat_qr_decompositionT_f32(&srckf->YCM, &srckf->SYT);
//estimate the cross-covariance matrix
for(col = 0; col < SRCKF_CP_POINTS; col++){
arm_mat_getcolumn_f32(&srckf->XCP, tmpX, col);
arm_mat_sub_f32(&srckf->tmpX, &srckf->X, &srckf->tmpX);
arm_mat_scale_f32(&srckf->tmpX, srckf->SW, &srckf->tmpX);
arm_mat_setcolumn_f32(&srckf->XCPCM, tmpX, col);
}
arm_mat_trans_f32(&srckf->YCPCM, &srckf->YCPCMT);
arm_mat_mult_f32(&srckf->XCPCM, &srckf->YCPCMT, &srckf->PXY);
//estimate the kalman gain
arm_mat_trans_f32(&srckf->SYT, &srckf->SY);
arm_mat_inverse_f32(&srckf->SYT, &srckf->SYTI);
arm_mat_inverse_f32(&srckf->SY, &srckf->SYI);
arm_mat_mult_f32(&srckf->PXY, &srckf->SYTI, &srckf->tmpPXY);
arm_mat_mult_f32(&srckf->tmpPXY, &srckf->SYI, &srckf->K);
//estimate the updated state
Y[0] = accel[0] - Y[0];
Y[1] = accel[1] - Y[1];
Y[2] = accel[2] - Y[2];
Y[3] = mag[0] - Y[3];
Y[4] = mag[1] - Y[4];
Y[5] = mag[2] - Y[5];
arm_mat_mult_f32(&srckf->K, &srckf->Y, &srckf->tmpX);
arm_mat_add_f32(&srckf->X, &srckf->tmpX, &srckf->X);
//normalize quaternion
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
//estimate the square-root factor of the corresponding error covariance
arm_mat_mult_f32(&srckf->K, &srckf->YCPCM, &srckf->tmpXCPCM);
arm_mat_sub_f32(&srckf->XCPCM, &srckf->tmpXCPCM, &srckf->XCPCM);
arm_mat_setsubmatrix_f32(&srckf->XYCM, &srckf->XCPCM, 0, 0);
arm_mat_mult_f32(&srckf->K, &srckf->SR, &srckf->tmpPXY);
arm_mat_setsubmatrix_f32(&srckf->XYCM, &srckf->tmpPXY, 0, SRCKF_CP_POINTS);
arm_mat_qr_decompositionT_f32(&srckf->XYCM, &srckf->ST);
arm_mat_trans_f32(&srckf->ST, &srckf->S);
}
