void EFK_Update(EKF_Filter* ekf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt) { float32_t norm; float32_t h[EKF_MEASUREMENT_DIM]; float32_t halfdx, halfdy, halfdz; float32_t neghalfdx, neghalfdy, neghalfdz; float32_t halfdtq0, halfdtq1, neghalfdtq1, halfdtq2, neghalfdtq2, halfdtq3, neghalfdtq3; float32_t halfdt = 0.5f * dt; #ifdef USE_4TH_RUNGE_KUTTA float32_t tmpW[4]; #endif ////////////////////////////////////////////////////////////////////////// float32_t q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; float32_t _2q0,_2q1,_2q2,_2q3; float32_t q0, q1, q2, q3; // float32_t hx, hy, hz; float32_t bx, bz; float32_t _2mx, _2my, _2mz; // float32_t *H = ekf->H_f32, *F = ekf->F_f32; float32_t *X = ekf->X_f32, *Y = ekf->Y_f32; ////////////////////////////////////////////////////////////////////////// halfdx = halfdt * X[4]; neghalfdx = -halfdx; halfdy = halfdt * X[5]; neghalfdy = -halfdy; halfdz = halfdt * X[6]; neghalfdz = -halfdz; // q0 = X[0]; q1 = X[1]; q2 = X[2]; q3 = X[3]; halfdtq0 = halfdt * q0; halfdtq1 = halfdt * q1; neghalfdtq1 = -halfdtq1; halfdtq2 = halfdt * q2; neghalfdtq2 = -halfdtq2; halfdtq3 = halfdt * q3; neghalfdtq3 = -halfdtq3; //F[0] = 1.0f; F[1] = neghalfdx; F[2] = neghalfdy; F[3] = neghalfdz; F[4] = neghalfdtq1; F[5] = neghalfdtq2; F[6] = neghalfdtq3; F[7] = halfdx; //F[8] = 1.0f; F[9] = neghalfdz; F[10] = halfdy; F[11] = halfdtq0; F[12] = halfdtq3; F[13] = neghalfdtq2; F[14] = halfdy; F[15] = halfdz; //F[16] = 1.0f; F[17] = neghalfdx; F[18] = neghalfdtq3; F[19] = halfdtq0; F[20] = neghalfdtq1; F[21] = halfdz; F[22] = neghalfdy; F[23] = halfdx; //F[24] = 1.0f; F[25] = halfdtq2; F[26] = neghalfdtq1; F[27] = halfdtq0; //model prediction //simple way, pay attention!!! //according to the actual gyroscope output //and coordinate system definition #ifdef USE_4TH_RUNGE_KUTTA tmpW[0] = 0; tmpW[1] = X[4]; tmpW[2] = X[5]; tmpW[3] = X[6]; Quaternion_RungeKutta4(X, tmpW, dt, 1); #else X[0] = q0 - (halfdx * q1 + halfdy * q2 + halfdz * q3); X[1] = q1 + (halfdx * q0 + halfdy * q3 - halfdz * q2); X[2] = q2 - (halfdx * q3 - halfdy * q0 - halfdz * q1); X[3] = q3 + (halfdx * q2 - halfdy * q1 + halfdz * q0); ////////////////////////////////////////////////////////////////////////// //Re-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; #endif //X covariance matrix update based on model //P = F*P*F' + Q; arm_mat_trans_f32(&ekf->F, &ekf->FT); arm_mat_mult_f32(&ekf->F, &ekf->P, &ekf->tmpP); arm_mat_mult_f32(&ekf->tmpP, &ekf->FT, &ekf->P); arm_mat_add_f32(&ekf->P, &ekf->Q, &ekf->tmpP); ////////////////////////////////////////////////////////////////////////// //model and measurement differences //normalize acc and mag measurements 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; //Auxiliary variables to avoid repeated arithmetic _2q0 = 2.0f * X[0]; _2q1 = 2.0f * X[1]; _2q2 = 2.0f * X[2]; _2q3 = 2.0f * X[3]; // q0q0 = X[0] * X[0]; q0q1 = X[0] * X[1]; q0q2 = X[0] * X[2]; q0q3 = X[0] * X[3]; q1q1 = X[1] * X[1]; q1q2 = X[1] * X[2]; q1q3 = X[1] * X[3]; q2q2 = X[2] * X[2]; q2q3 = X[2] * X[3]; q3q3 = X[3] * X[3]; _2mx = 2.0f * mag[0]; _2my = 2.0f * mag[1]; _2mz = 2.0f * mag[2]; //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; h[0] = X[0]; h[1] = X[1]; h[2] = X[2]; h[3] = X[3]; h[4] = 2.0f * (q1q3 - q0q2); h[5] = 2.0f * (q2q3 + q0q1); h[6] = -1.0f + 2.0f * (q0q0 + q3q3); h[7] = X[4]; h[8] = X[5]; h[9] = X[6]; h[10] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2)); h[11] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1)); h[12] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2)); ///////////////////////////////////////////////////////////////////////// //The H matrix maps the measurement to the states 13 x 7 //row started from 0 to 12, col started from 0 to 6 //row 4, col 0~3 H[28] = -_2q2; H[29] = _2q3; H[30] = -_2q0; H[31] = _2q1; //row 5, col 0~3 H[35] = _2q1; H[36] = _2q0; H[37] = _2q3; H[38] = _2q2; //row 6, col 0~3 H[42] = _2q0; H[43] = -_2q1; H[44] = -_2q2; H[45] = _2q3; //row 10, col 0~3 H[70] = bx * _2q0 - bz * _2q2; H[71] = bx * _2q1 + bz * _2q3; H[72] = -bx * _2q2 - bz * _2q0; H[73] = bz * _2q1 - bx * _2q3; //row 11, col 0~3 H[77] = bz * _2q1 - bx * _2q3; H[78] = bx * _2q2 + bz * _2q0; H[79] = bx * _2q1 + bz * _2q3; H[80] = bz * _2q2 - bx * _2q0; //row 12, col 0~3 H[84] = bx * _2q2 + bz * _2q0; H[85] = bx * _2q3 - bz * _2q1; H[86] = bx * _2q0 - bz * _2q2; H[87] = bx * _2q1 + bz * _2q3; // //y = z - h; Y[0] = q[0] - h[0]; Y[1] = q[1] - h[1]; Y[2] = q[2] - h[2]; Y[3] = q[3] - h[3]; // Y[4] = accel[0] - h[4]; Y[5] = accel[1] - h[5]; Y[6] = accel[2] - h[6]; Y[7] = gyro[0] - h[7]; Y[8] = gyro[1] - h[8]; Y[9] = gyro[2] - h[9]; ////////////////////////////////////////////////////////////////////////// Y[10] = mag[0] - h[10]; Y[11] = mag[1] - h[11]; Y[12] = mag[2] - h[12]; ////////////////////////////////////////////////////////////////////////// //Measurement covariance update //S = H*P*H' + R; arm_mat_trans_f32(&ekf->H, &ekf->HT); arm_mat_mult_f32(&ekf->H, &ekf->tmpP, &ekf->tmpYX); arm_mat_mult_f32(&ekf->tmpYX, &ekf->HT, &ekf->S); arm_mat_add_f32(&ekf->S, &ekf->R, &ekf->tmpS); //Calculate Kalman gain //K = P*H'/S; arm_mat_inverse_f32(&ekf->tmpS, &ekf->S); arm_mat_mult_f32(&ekf->tmpP, &ekf->HT, &ekf->tmpXY); arm_mat_mult_f32(&ekf->tmpXY, &ekf->S, &ekf->K); //Corrected model prediction //S = S + K*y; arm_mat_mult_f32(&ekf->K, &ekf->Y, &ekf->tmpX); arm_mat_add_f32(&ekf->X, &ekf->tmpX, &ekf->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; //Update state covariance with new knowledge //option: P = P - K*H*P or P = (I - K*H)*P*(I - K*H)' + K*R*K' #if 0 //P = P - K*H*P //simple but it can't ensure the matrix will be a positive definite matrix arm_mat_mult_f32(&ekf->K, &ekf->H, &ekf->tmpXX); arm_mat_mult_f32(&ekf->tmpXX, &ekf->tmpP, &ekf->P); arm_mat_sub_f32(&ekf->tmpP, &ekf->P, &ekf->P); #else //P=(I - K*H)*P*(I - K*H)' + K*R*K' arm_mat_mult_f32(&ekf->K, &ekf->H, &ekf->tmpXX); arm_mat_sub_f32(&ekf->I, &ekf->tmpXX, &ekf->tmpXX); arm_mat_trans_f32(&ekf->tmpXX, &ekf->tmpXXT); arm_mat_mult_f32(&ekf->tmpXX, &ekf->tmpP, &ekf->P); arm_mat_mult_f32(&ekf->P, &ekf->tmpXXT, &ekf->tmpP); arm_mat_trans_f32(&ekf->K, &ekf->KT); arm_mat_mult_f32(&ekf->K, &ekf->R, &ekf->tmpXY); arm_mat_mult_f32(&ekf->tmpXY, &ekf->KT, &ekf->tmpXX); arm_mat_add_f32(&ekf->tmpP, &ekf->tmpXX, &ekf->P); #endif }
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); }