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;
}
