void IMU::computeYaw(void) { float32_t sqrt_result, arg1, arg2; float32_t mag[3]; mag[0] = magnetometer.axis_f[0]; mag[1] = magnetometer.axis_f[1]; mag[2] = magnetometer.axis_f[2]; //Normalise measurement: arg1 = (float32_t) (mag[0]) * (mag[0]) + (mag[1]) * (mag[1]) + (mag[2]) * (mag[2]); arm_sqrt_f32(arg1, &sqrt_result); mag[0] /= sqrt_result; mag[1] /= sqrt_result; mag[2] /= sqrt_result; arg1 = arm_sin_f32(XRollAngleInRad) * mag[2] - arm_cos_f32(XRollAngleInRad) * mag[1]; arg2 = arm_cos_f32(YPitchAngleInRad) * mag[0] + arm_sin_f32(XRollAngleInRad) * arm_sin_f32(YPitchAngleInRad) * mag[1] + arm_cos_f32(XRollAngleInRad) * arm_sin_f32(YPitchAngleInRad) * mag[2]; ZYawAngleInRad = atan2f(arg1, arg2); //Correct declination, and change to compass angles // ZYawAngleInRad += (5.0 + (16.0 / 60.0))*PI/180.0; //Positive declination. // if (ZYawAngleInRad < 0) { // ZYawAngleInRad += 2 * PI; // } else if (ZYawAngleInRad > 2 * PI) { // ZYawAngleInRad -= 2 * PI; // } }
void IMU::removeCentrifugalForceEffect(void) { float32_t arg1 = .0, sumOfSquares = .0; float32_t result = .0; volatile float32_t centrifugalAcceleration[3] = { .0 }; float32_t rotationThresholdInRadPerSec = 5.0; //Physical dimensions on board in mm const float32_t dx = 0.015, //15 mm dy = 0.01, //20 mm dz = 0.003; //3 mm float32_t *const pGyroAxis = gyro.axisInRadPerS, *const pAccAxis = accelerometer.axisInMPerSsquared; if (pGyroAxis[0] > rotationThresholdInRadPerSec) { centrifugalAcceleration[1] = pGyroAxis[0] * pGyroAxis[0] * dx; } if (pGyroAxis[1] > rotationThresholdInRadPerSec) { centrifugalAcceleration[0] = pGyroAxis[1] * pGyroAxis[1] * dy; } if (pGyroAxis[2] > rotationThresholdInRadPerSec) { arm_sqrt_f32(sumOfSquares, &result); centrifugalAcceleration[0] = pGyroAxis[2] * pGyroAxis[2] * result; centrifugalAcceleration[1] = pGyroAxis[2] * pGyroAxis[2] * result; sumOfSquares = dx * dx + dy * dy; arg1 = dy / sumOfSquares; centrifugalAcceleration[0] *= arm_sin_f32(arg1); arg1 = dx / sumOfSquares; centrifugalAcceleration[1] *= arm_cos_f32(arg1); } pAccAxis[0] -= centrifugalAcceleration[0]; pAccAxis[1] -= centrifugalAcceleration[1]; pAccAxis[2] -= centrifugalAcceleration[2]; }
void arm_rms_f32( float32_t * pSrc, uint32_t blockSize, float32_t * pResult) { float32_t sum = 0.0f; /* Accumulator */ float32_t in; /* Tempoprary variable to store input value */ uint32_t blkCnt; /* loop counter */ #ifndef ARM_MATH_CM0 /* Run the below code for Cortex-M4 and Cortex-M3 */ /* loop Unrolling */ blkCnt = blockSize >> 2u; /* First part of the processing with loop unrolling. Compute 4 outputs at a time. ** a second loop below computes the remaining 1 to 3 samples. */ while(blkCnt > 0u) { /* C = A[0] * A[0] + A[1] * A[1] + A[2] * A[2] + ... + A[blockSize-1] * A[blockSize-1] */ /* Compute sum of the squares and then store the result in a temporary variable, sum */ in = *pSrc++; sum += in * in; in = *pSrc++; sum += in * in; in = *pSrc++; sum += in * in; in = *pSrc++; sum += in * in; /* Decrement the loop counter */ blkCnt--; } /* If the blockSize is not a multiple of 4, compute any remaining output samples here. ** No loop unrolling is used. */ blkCnt = blockSize % 0x4u; #else /* Run the below code for Cortex-M0 */ /* Loop over blockSize number of values */ blkCnt = blockSize; #endif /* #ifndef ARM_MATH_CM0 */ while(blkCnt > 0u) { /* C = A[0] * A[0] + A[1] * A[1] + A[2] * A[2] + ... + A[blockSize-1] * A[blockSize-1] */ /* Compute sum of the squares and then store the results in a temporary variable, sum */ in = *pSrc++; sum += in * in; /* Decrement the loop counter */ blkCnt--; } /* Compute Rms and store the result in the destination */ arm_sqrt_f32(sum / (float32_t) blockSize, pResult); }
/* Do FFT for 4096 points of data. * * Return the frequency which has the max response. */ int fft_4096( int16_t *data ) { int i, maxIndex; float maxResponse = 0.0; fft_input_init( data ); bit_reversal(); butterfly(); // Magnitude of complex number for ( i = 0; i < DATA_LENGTH; ++i ) { arm_sqrt_f32( fft_input[i].real * fft_input[i].real + fft_input[i].imag * fft_input[i].imag, &fft_input[i].real ); fft_input[i].real /= ( i == 0 ? DATA_LENGTH : DATA_LENGTH / 2 ); } // Find the max response index // Only care about the first half of the fft output for ( i = 0; i < HALF_DATA_LENGTH; ++i ) { if ( fft_input[i].real > maxResponse ) { maxResponse = fft_input[i].real; maxIndex = i; } } return (int)(2000 / 2 / 4096 * maxIndex); } // end of fft_4096()
void vector_norm(const vec_3d *a, float *n) { dot_prod(a, a, n); #ifdef LIBQUAT_ARM arm_sqrt_f32(*n, n); #else *n = sqrtf(*n); #endif }
/* * Funktion gibt den Betrag des Vektors in uT, den Winkel alpha des auf die XY Ebene projektierten Vektors * bezüglich der X-Achse und den Winkel beta des Vektors bezüglich der XY Ebene zurück */ void get_vektor(magnVec_t *vector) { float32_t vector_uT[3]; float32_t temp,temp2; get_magn_uT(vector_uT); #if distance_assignment vector->x_val = vector_uT[0]; vector->y_val = vector_uT[1]; vector->z_val = vector_uT[2]; #endif //Betrag berechnen: temp = fabs(vector_uT[0]*vector_uT[0])+(vector_uT[1]*vector_uT[1])+(vector_uT[2]*vector_uT[2]); arm_sqrt_f32(temp,&vector->value); //Winkel alpha muss angepasst werden nach Quadrant: if(vector_uT[0]>0 && vector_uT[1]>0) //Vektor im 1. Quadranten { vector->alpha=((atanf(vector_uT[1]/vector_uT[0]))*180/PI); //*alpha=1; } else if(vector_uT[0]<0 && vector_uT[1]>0) //Vektor im 2. Quadranten { vector->alpha=((atanf(vector_uT[1]/vector_uT[0]))*180/PI)+180; //*alpha=2; } else if(vector_uT[0]<0 && vector_uT[1]<0) //Vektor im 3. Quadranten { vector->alpha=((atanf(vector_uT[1]/vector_uT[0]))*180/PI)+180; //*alpha=3; } else if(vector_uT[0]>0 && vector_uT[1]<0) //Vektor im 4. Quadranten { vector->alpha=((atanf(vector_uT[1]/vector_uT[0]))*180/PI)+360; //*alpha=4; } //beta berechnen: temp = fabs(vector_uT[0]*vector_uT[0]+vector_uT[1]*vector_uT[1]); arm_sqrt_f32(temp,&temp2); vector->beta=(atanf(vector_uT[2]/(temp2))*180/PI); }
void IMU::computePitchRollTilt(void) { float32_t arg1; float32_t sqrt_result; float32_t acc[3]; acc[0] = accelerometer.axisInMPerSsquared[0]; acc[1] = accelerometer.axisInMPerSsquared[1]; acc[2] = accelerometer.axisInMPerSsquared[2]; //Normalise measurement: arg1 = (float32_t) (acc[0]) * (acc[0]) + (acc[1]) * (acc[1]) + (acc[2]) * (acc[2]); arm_sqrt_f32(arg1, &sqrt_result); acc[0] /= sqrt_result; acc[1] /= sqrt_result; acc[2] /= sqrt_result; int8_t signZ; const float32_t mi = 0.01; //Compute angles: //wzor jest nieprawdziwy, gdy z=0 i y=0. nie ma jednego dobreg rozwiazania. Mozna np. aproksymowac w ten sposob: if (acc[2] > 0) { signZ = 1; } else { signZ = -1; } arg1 = (acc[2]) * (acc[2]) + mi * (acc[0]) * (acc[0]); arm_sqrt_f32(arg1, &sqrt_result); XRollAngleInRad = atan2(acc[1], signZ * sqrt_result); arg1 = 0; arg1 += (float32_t) (acc[2]) * (float32_t) (acc[2]); arg1 += (float32_t) (acc[1]) * (float32_t) (acc[1]); arm_sqrt_f32(arg1, &sqrt_result); YPitchAngleInRad = atan2(-(acc[0]), sqrt_result); //YPitchAngleInRad_2 = arm_sin_f32((float32_t)(-acc[0])/(float32_t)(g)); arg1 = 0; arg1 += (float32_t) (acc[0]) * (float32_t) (acc[0]); arg1 += (float32_t) (acc[1]) * (float32_t) (acc[1]); arg1 += (float32_t) (acc[2]) * (float32_t) (acc[2]); arm_sqrt_f32(arg1, &sqrt_result); TiltAngleInRad = arm_cos_f32((acc[2]) / sqrt_result); }
float getVectorModulus(const float vector[], u8 numberOfElements) { u8 i = 0; float temp = 0.0; for(i = 0; i < numberOfElements; i++) { temp += powf(vector[i], 2); } arm_sqrt_f32(temp, &temp); return temp; }
//The following method takes an input vector and its length, and calculates its standard deviation void c_std(float* input_vector, int vector_length, float* vector_std) { float sumOfSquares = 0; float sum = 0; int i = 0; while (i < vector_length) { sumOfSquares += input_vector[i] * input_vector[i]; sum += input_vector[i]; i++; } arm_sqrt_f32(((sumOfSquares - (sum * sum) / vector_length) / (vector_length - 1)), vector_std); }
void arm_cmplx_mag_f32( float32_t * pSrc, float32_t * pDst, uint32_t numSamples) { float32_t realIn, imagIn; /* Temporary variables to hold input values */ #ifndef ARM_MATH_CM0_FAMILY /* Run the below code for Cortex-M4 and Cortex-M3 */ uint32_t blkCnt; /* loop counter */ /*loop Unrolling */ blkCnt = numSamples >> 2u; /* First part of the processing with loop unrolling. Compute 4 outputs at a time. ** a second loop below computes the remaining 1 to 3 samples. */ while(blkCnt > 0u) { /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ realIn = *pSrc++; imagIn = *pSrc++; /* store the result in the destination buffer. */ arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++); realIn = *pSrc++; imagIn = *pSrc++; arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++); realIn = *pSrc++; imagIn = *pSrc++; arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++); realIn = *pSrc++; imagIn = *pSrc++; arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++); /* Decrement the loop counter */ blkCnt--; } /* If the numSamples is not a multiple of 4, compute any remaining output samples here. ** No loop unrolling is used. */ blkCnt = numSamples % 0x4u; while(blkCnt > 0u) { /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ realIn = *pSrc++; imagIn = *pSrc++; /* store the result in the destination buffer. */ arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++); /* Decrement the loop counter */ blkCnt--; } #else /* Run the below code for Cortex-M0 */ while(numSamples > 0u) { /* out = sqrt((real * real) + (imag * imag)) */ realIn = *pSrc++; imagIn = *pSrc++; /* store the result in the destination buffer. */ arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++); /* Decrement the loop counter */ numSamples--; } #endif /* #ifndef ARM_MATH_CM0_FAMILY */ }
void UKF_New(UKF_Filter* ukf) { float32_t *P = ukf->P_f32; float32_t *Q = ukf->Q_f32; float32_t *R = ukf->R_f32; float32_t *Wc = ukf->Wc_f32; //scaling factor float32_t lambda = UKF_alpha * UKF_alpha *((float32_t)UKF_STATE_DIM + UKF_kappa) - (float32_t)UKF_STATE_DIM; float32_t gamma = (float32_t)UKF_STATE_DIM + lambda; //weights for means ukf->Wm0 = lambda / gamma; ukf->Wmi = 0.5f / gamma; //weights for covariance arm_mat_init_f32(&ukf->Wc, UKF_SP_POINTS, UKF_SP_POINTS, ukf->Wc_f32); arm_mat_identity_f32(&ukf->Wc, ukf->Wmi); Wc[0] = ukf->Wm0 + (1.0f - UKF_alpha * UKF_alpha + UKF_beta); //scaling factor need to tune! arm_sqrt_f32(gamma, &ukf->gamma); ////////////////////////////////////////////////////////////////////////// //initialise P arm_mat_init_f32(&ukf->P, UKF_STATE_DIM, UKF_STATE_DIM, ukf->P_f32); arm_mat_identity_f32(&ukf->P, 1.0f); P[0] = P[8] = P[16] = P[24] = UKF_PQ_INITIAL; P[32] = P[40] = P[48] = UKF_PW_INITIAL; arm_mat_init_f32(&ukf->PX, UKF_STATE_DIM, UKF_STATE_DIM, ukf->PX_f32); arm_mat_init_f32(&ukf->PY, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->PY_f32); arm_mat_init_f32(&ukf->tmpPY, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->tmpPY_f32); arm_mat_init_f32(&ukf->PXY, UKF_STATE_DIM, UKF_MEASUREMENT_DIM, ukf->PXY_f32); arm_mat_init_f32(&ukf->PXYT, UKF_MEASUREMENT_DIM, UKF_STATE_DIM, ukf->PXYT_f32); //initialise Q arm_mat_init_f32(&ukf->Q, UKF_STATE_DIM, UKF_STATE_DIM, ukf->Q_f32); Q[0] = Q[8] = Q[16] = Q[24] = UKF_QQ_INITIAL; Q[32] = Q[40] = Q[48] = UKF_QW_INITIAL; //initialise R arm_mat_init_f32(&ukf->R, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->R_f32); R[0] = R[14] = R[28] = R[42] = UKF_RQ_INITIAL; R[56] = R[70] = R[84] = UKF_RA_INITIAL; R[98] = R[112] = R[126] = UKF_RW_INITIAL; R[140] = R[154] = R[168] = UKF_RM_INITIAL; arm_mat_init_f32(&ukf->XSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->XSP_f32); arm_mat_init_f32(&ukf->tmpXSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->tmpXSP_f32); arm_mat_init_f32(&ukf->tmpXSPT, UKF_SP_POINTS, UKF_STATE_DIM, ukf->tmpXSPT_f32); arm_mat_init_f32(&ukf->tmpWcXSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->tmpWcXSP_f32); arm_mat_init_f32(&ukf->tmpWcYSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->tmpWcYSP_f32); arm_mat_init_f32(&ukf->YSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->YSP_f32); arm_mat_init_f32(&ukf->tmpYSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->tmpYSP_f32); arm_mat_init_f32(&ukf->tmpYSPT, UKF_SP_POINTS, UKF_MEASUREMENT_DIM, ukf->tmpYSPT_f32); ////////////////////////////////////////////////////////////////////////// arm_mat_init_f32(&ukf->K, UKF_STATE_DIM, UKF_MEASUREMENT_DIM, ukf->K_f32); arm_mat_init_f32(&ukf->KT, UKF_MEASUREMENT_DIM, UKF_STATE_DIM, ukf->KT_f32); ////////////////////////////////////////////////////////////////////////// arm_mat_init_f32(&ukf->X, UKF_STATE_DIM, 1, ukf->X_f32); arm_mat_zero_f32(&ukf->X); arm_mat_init_f32(&ukf->tmpX, UKF_STATE_DIM, 1, ukf->tmpX_f32); arm_mat_zero_f32(&ukf->tmpX); ////////////////////////////////////////////////////////////////////////// arm_mat_init_f32(&ukf->Y, UKF_MEASUREMENT_DIM, 1, ukf->Y_f32); arm_mat_zero_f32(&ukf->Y); arm_mat_init_f32(&ukf->tmpY, UKF_MEASUREMENT_DIM, 1, ukf->tmpY_f32); arm_mat_zero_f32(&ukf->tmpY); ////////////////////////////////////////////////////////////////////////// }
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_New(SRCKF_Filter* srckf) { float32_t kesi; arm_matrix_instance_f32 KesiPuls, KesiMinu; float32_t KesiPuls_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM], KesiMinus_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM]; float32_t *S = srckf->S_f32; float32_t *SQ = srckf->SQ_f32; float32_t *SR = srckf->SR_f32; ////////////////////////////////////////////////////////////////////////// //initialise weight srckf->W = 1.0f / (float32_t)SRCKF_CP_POINTS; arm_sqrt_f32(srckf->W, &srckf->SW); //initialise kesi //generate the cubature point kesi = (float32_t)SRCKF_STATE_DIM; arm_sqrt_f32(kesi, &kesi); arm_mat_init_f32(&KesiPuls, SRCKF_STATE_DIM, SRCKF_STATE_DIM, KesiPuls_f32); arm_mat_init_f32(&KesiMinu, SRCKF_STATE_DIM, SRCKF_STATE_DIM, KesiMinus_f32); arm_mat_identity_f32(&KesiPuls, kesi); arm_mat_identity_f32(&KesiMinu, -kesi); arm_mat_init_f32(&srckf->Kesi, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->Kesi_f32); arm_mat_setsubmatrix_f32(&srckf->Kesi, &KesiPuls, 0, 0); arm_mat_setsubmatrix_f32(&srckf->Kesi, &KesiMinu, 0, SRCKF_STATE_DIM); arm_mat_init_f32(&srckf->iKesi, SRCKF_STATE_DIM, 1, srckf->iKesi_f32); //initialise state covariance arm_mat_init_f32(&srckf->S, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->S_f32); arm_mat_zero_f32(&srckf->S); S[0] = S[8] = S[16] = S[24] = SRCKF_PQ_INITIAL; S[32] = S[40] = S[48] = SRCKF_PW_INITIAL; arm_mat_init_f32(&srckf->ST, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->ST_f32); //initialise measurement covariance arm_mat_init_f32(&srckf->SY, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SY_f32); arm_mat_init_f32(&srckf->SYI, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYI_f32); arm_mat_init_f32(&srckf->SYT, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYT_f32); arm_mat_init_f32(&srckf->SYTI, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYTI_f32); arm_mat_init_f32(&srckf->PXY, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->PXY_f32); arm_mat_init_f32(&srckf->tmpPXY, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->tmpPXY_f32); //initialise SQ arm_mat_init_f32(&srckf->SQ, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->SQ_f32); arm_mat_zero_f32(&srckf->SQ); SQ[0] = SQ[8] = SQ[16] = SQ[24] = SRCKF_QQ_INITIAL; SQ[32] = SQ[40] = SQ[48] = SRCKF_QW_INITIAL; //initialise SR arm_mat_init_f32(&srckf->SR, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SR_f32); arm_mat_zero_f32(&srckf->SR); SR[0] = SR[7] = SR[14] = SRCKF_RA_INITIAL; SR[21] = SR[28] = SR[35] = SRCKF_RM_INITIAL; //other stuff //cubature points arm_mat_init_f32(&srckf->XCP, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XCP_f32); //propagated cubature points arm_mat_init_f32(&srckf->XPCP, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XPCP_f32); arm_mat_init_f32(&srckf->YPCP, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS, srckf->YPCP_f32); arm_mat_init_f32(&srckf->XCPCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XCPCM_f32); arm_mat_init_f32(&srckf->tmpXCPCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->tmpXCPCM_f32); arm_mat_init_f32(&srckf->YCPCM, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS, srckf->YCPCM_f32); arm_mat_init_f32(&srckf->YCPCMT, SRCKF_CP_POINTS, SRCKF_MEASUREMENT_DIM, srckf->YCPCMT_f32); arm_mat_init_f32(&srckf->XCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS + SRCKF_STATE_DIM, srckf->XCM_f32); //initialise fill SQ arm_mat_setsubmatrix_f32(&srckf->XCM, &srckf->SQ, 0, SRCKF_CP_POINTS); arm_mat_init_f32(&srckf->YCM, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM, srckf->YCM_f32); //initialise fill SR arm_mat_setsubmatrix_f32(&srckf->YCM, &srckf->SR, 0, SRCKF_CP_POINTS); arm_mat_init_f32(&srckf->XYCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM, srckf->XYCM_f32); //Kalman gain arm_mat_init_f32(&srckf->K, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->K_f32); ////////////////////////////////////////////////////////////////////////// //state vector arm_mat_init_f32(&srckf->X, SRCKF_STATE_DIM, 1, srckf->X_f32); arm_mat_zero_f32(&srckf->X); //measurement vector arm_mat_init_f32(&srckf->Y, SRCKF_MEASUREMENT_DIM, 1, srckf->Y_f32); arm_mat_zero_f32(&srckf->Y); arm_mat_init_f32(&srckf->tmpY, SRCKF_MEASUREMENT_DIM, 1, srckf->tmpY_f32); arm_mat_zero_f32(&srckf->tmpY); }
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 arm_std_f32( float32_t * pSrc, uint32_t blockSize, float32_t * pResult) { float32_t sum = 0.0f; /* Temporary result storage */ float32_t sumOfSquares = 0.0f; /* Sum of squares */ float32_t in; /* input value */ uint32_t blkCnt; /* loop counter */ #ifndef ARM_MATH_CM0_FAMILY /* Run the below code for Cortex-M4 and Cortex-M3 */ float32_t meanOfSquares, mean, squareOfMean; if(blockSize == 1) { *pResult = 0; return; } /*loop Unrolling */ blkCnt = blockSize >> 2u; /* First part of the processing with loop unrolling. Compute 4 outputs at a time. ** a second loop below computes the remaining 1 to 3 samples. */ while(blkCnt > 0u) { /* C = (A[0] * A[0] + A[1] * A[1] + ... + A[blockSize-1] * A[blockSize-1]) */ /* Compute Sum of squares of the input samples * and then store the result in a temporary variable, sum. */ in = *pSrc++; sum += in; sumOfSquares += in * in; in = *pSrc++; sum += in; sumOfSquares += in * in; in = *pSrc++; sum += in; sumOfSquares += in * in; in = *pSrc++; sum += in; sumOfSquares += in * in; /* Decrement the loop counter */ blkCnt--; } /* If the blockSize is not a multiple of 4, compute any remaining output samples here. ** No loop unrolling is used. */ blkCnt = blockSize % 0x4u; while(blkCnt > 0u) { /* C = (A[0] * A[0] + A[1] * A[1] + ... + A[blockSize-1] * A[blockSize-1]) */ /* Compute Sum of squares of the input samples * and then store the result in a temporary variable, sum. */ in = *pSrc++; sum += in; sumOfSquares += in * in; /* Decrement the loop counter */ blkCnt--; } /* Compute Mean of squares of the input samples * and then store the result in a temporary variable, meanOfSquares. */ meanOfSquares = sumOfSquares / ((float32_t) blockSize - 1.0f); /* Compute mean of all input values */ mean = sum / (float32_t) blockSize; /* Compute square of mean */ squareOfMean = (mean * mean) * (((float32_t) blockSize) / ((float32_t) blockSize - 1.0f)); /* Compute standard deviation and then store the result to the destination */ arm_sqrt_f32((meanOfSquares - squareOfMean), pResult); #else /* Run the below code for Cortex-M0 */ float32_t squareOfSum; /* Square of Sum */ float32_t var; /* Temporary varaince storage */ if(blockSize == 1) { *pResult = 0; return; } /* Loop over blockSize number of values */ blkCnt = blockSize; while(blkCnt > 0u) { /* C = (A[0] * A[0] + A[1] * A[1] + ... + A[blockSize-1] * A[blockSize-1]) */ /* Compute Sum of squares of the input samples * and then store the result in a temporary variable, sumOfSquares. */ in = *pSrc++; sumOfSquares += in * in; /* C = (A[0] + A[1] + ... + A[blockSize-1]) */ /* Compute Sum of the input samples * and then store the result in a temporary variable, sum. */ sum += in; /* Decrement the loop counter */ blkCnt--; } /* Compute the square of sum */ squareOfSum = ((sum * sum) / (float32_t) blockSize); /* Compute the variance */ var = ((sumOfSquares - squareOfSum) / (float32_t) (blockSize - 1.0f)); /* Compute standard deviation and then store the result to the destination */ arm_sqrt_f32(var, pResult); #endif /* #ifndef ARM_MATH_CM0_FAMILY */ }
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 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); }
static void GPS_ISR() { TIM_ClearITPendingBit(GPS_TIMER, TIM_IT_Update); double l1 = (double)GetEncoder1Count() * Encoder1_Fact * Encoder1_DIR; ClearEncoder1Count(); double l2 = (double)GetEncoder2Count() * Encoder2_Fact * Encoder2_DIR; ClearEncoder2Count(); #ifndef TWO_ENCODERS double l3 = (double)GetEncoder3Count() * Encoder3_Fact * Encoder3_DIR; ClearEncoder3Count(); #endif static double lastV = 0; #ifdef TWO_ENCODERS if ( ABS(l1) < ROTATE_MESURE && ABS(l2) < ROTATE_MESURE) { g_isRotate = FALSE; } else { g_isRotate = TRUE; } #else if ( ABS(l1 - l3) < ROTATE_MESURE ) { g_isRotate = FALSE; } else { g_isRotate = TRUE; } #endif static double LastDeg = 0; double currentDeg = CalculateGyroDeg(); double deltaDeg = currentDeg - LastDeg ; double degData = (LastDeg + currentDeg); if (deltaDeg > 180 ) { deltaDeg -= 360; degData += 360; } else if (deltaDeg < -180) { deltaDeg += 360; degData += 360; } g_AnglurSpeed = deltaDeg * 200; l1 -= deltaDeg * Encoder1_RotateFact; l2 -= deltaDeg * Encoder2_RotateFact; float _ts = l1 * l1 + l2 * l2; arm_sqrt_f32(_ts,&_ts); g_linerSpeed = _ts * 200; lastV = g_linerSpeed; #ifndef TWO_ENCODERS l3 -= deltaDeg * Encoder3_RotateFact; l1 = (l1 + l3) / 2.0; #endif //计算等效的平移方向 // degData = degData *0.5f + ANGLE_C; degData *= DEG2RAD; LastDeg = currentDeg; float degCosData, degSinData; degCosData = arm_cos_f32(degData); degSinData = arm_sin_f32(degData); double deltaX = l2 * degCosData - l1 * degSinData ; double deltaY = l1 * degCosData + l2 * degSinData ; g_NowlinerSpeedAngle = atan2(deltaY , deltaX) * RAD2DEG; g_X += deltaX; g_Y += deltaY; }