void paramsrcdkfUpdate(srcdkf_t *f, float32_t *u, float32_t *d) {
	int S = f->S;
	float32_t *Sx = f->Sx.pData;
	float32_t *Sv = f->Sv.pData;
	float32_t *rDiag = f->rDiag.pData;
	int i;

	srcdkfMeasurementUpdate(f, u, d, f->M, f->N, 0, f->map);

	if (f->rm) {
		// Robbins-Monro innovation estimation for Rr
		// Rr = (1 - SRCDKF_RM)*Rr + SRCDKF_RM * K * [dk - g(xk, wk)] * [dk - g(xk, wk)]^T * K^T

		arm_mat_trans_f32(&f->K, &f->KT);
		arm_mat_trans_f32(&f->inov, &f->inovT);

		// xUpdate == K*inov
		arm_mat_mult_f32(&f->xUpdate, &f->inovT, &f->K);
		arm_mat_mult_f32(&f->K, &f->KT, &f->Sv);

		for (i = 0; i < S; i++) {
			rDiag[i] = (1.0 - f->rm)*rDiag[i] + f->rm * Sv[i*S + i];
			Sx[i*S + i] = sqrt(Sx[i*S + i] * Sx[i*S + i] + rDiag[i]);
		}
	}
}
Beispiel #2
0
void srcdkfTimeUpdate(srcdkf_t *f, float32_t *u, float32_t dt) {
	int S = f->S;			// number of states
	int V = f->V;			// number of noise variables
	int L;				// number of sigma points
	float32_t *x = f->x.pData;	// state estimate
//	float32_t *Sv = f->Sv.pData;	// process covariance
	float32_t *Xa = f->Xa.pData;	// augmented sigma points
	float32_t *xIn = f->xIn;	// callback buffer
	float32_t *xOut = f->xOut;	// callback buffer
	float32_t *xNoise = f->xNoise;	// callback buffer
	float32_t *qrTempS = f->qrTempS.pData;
	int i=0, j=0;

	srcdkfCalcSigmaPoints(f, &f->Sv);
	L = f->L;

	// Xa = f(Xx, Xv, u, dt)
	for (i = 0; i < L; i++) {
		for (j = 0; j < S; j++)
			xIn[j] = Xa[j*L + i];

		for (j = 0; j < V; j++)
			xNoise[j] = Xa[(S+j)*L + i];

		f->timeUpdate(xIn, xNoise, xOut, u, dt, false);

		for (j = 0; j < S; j++)
			Xa[j*L + i] = xOut[j];
	}

	// sum weighted resultant sigma points to create estimated state
	f->w0m = (f->hh - (float32_t)(S+V)) / f->hh;
	for (i = 0; i < S; i++) {
		int rOffset = i*L;

		x[i] = Xa[rOffset + 0] * f->w0m;

		for (j = 1; j < L; j++)
			x[i] += Xa[rOffset + j] * f->wim;
	}

	// update state covariance
	for (i = 0; i < S; i++) {
		int rOffset = i*(S+V)*2;

		for (j = 0; j < S+V; j++) {
			qrTempS[rOffset + j] = (Xa[i*L + j + 1] - Xa[i*L + S+V + j + 1]) * f->wic1;
			qrTempS[rOffset + S+V + j] = (Xa[i*L + j + 1] + Xa[i*L + S+V + j + 1] - 2.0f*Xa[i*L + 0]) * f->wic2;
		}
	}
//matrixDump("qrTempS", &f->qrTempS);
//yield(1);
	qrDecompositionT_f32(&f->qrTempS, NULL, &f->SxT);   // with transposition
//matrixDump("SxT", &f->SxT);
//yield(200);
	arm_mat_trans_f32(&f->SxT, &f->Sx);
}
Beispiel #3
0
	/**
	 * transpose the matrix
	 */
	Matrix<N, M> transposed(void) const {
#ifdef CONFIG_ARCH_ARM
		Matrix<N, M> res;
		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
		return res;
#else
		matrix::Matrix<float, N, M> Me(this->arm_mat.pData);
		Matrix<N, M> res(Me.transpose().data());
		return res;
#endif
	}
Beispiel #4
0
	/**
	 * transpose the matrix
	 */
	Matrix<N, M> transposed(void) const {
#ifdef CONFIG_ARCH_ARM
		Matrix<N, M> res;
		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
		return res;
#else
		Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
				(this->arm_mat.pData);
		Me.transposeInPlace();
		Matrix<N, M> res(Me.data());
		return res;
#endif
	}
int32_t main(void)
{

  arm_matrix_instance_f32 A;      /* Matrix A Instance */
  arm_matrix_instance_f32 AT;     /* Matrix AT(A transpose) instance */
  arm_matrix_instance_f32 ATMA;   /* Matrix ATMA( AT multiply with A) instance */
  arm_matrix_instance_f32 ATMAI;  /* Matrix ATMAI(Inverse of ATMA) instance */
  arm_matrix_instance_f32 B;      /* Matrix B instance */
  arm_matrix_instance_f32 X;      /* Matrix X(Unknown Matrix) instance */

  uint32_t srcRows, srcColumns;  /* Temporary variables */
  arm_status status;

  /* Initialise A Matrix Instance with numRows, numCols and data array(A_f32) */
  srcRows = 4;
  srcColumns = 4;
  arm_mat_init_f32(&A, srcRows, srcColumns, (float32_t *)A_f32);

  /* Initialise Matrix Instance AT with numRows, numCols and data array(AT_f32) */
  srcRows = 4;
  srcColumns = 4;
  arm_mat_init_f32(&AT, srcRows, srcColumns, AT_f32);

  /* calculation of A transpose */
  status = arm_mat_trans_f32(&A, &AT);


  /* Initialise ATMA Matrix Instance with numRows, numCols and data array(ATMA_f32) */
  srcRows = 4;
  srcColumns = 4;
  arm_mat_init_f32(&ATMA, srcRows, srcColumns, ATMA_f32);

  /* calculation of AT Multiply with A */
  status = arm_mat_mult_f32(&AT, &A, &ATMA);

  /* Initialise ATMAI Matrix Instance with numRows, numCols and data array(ATMAI_f32) */
  srcRows = 4;
  srcColumns = 4;
  arm_mat_init_f32(&ATMAI, srcRows, srcColumns, ATMAI_f32);

  /* calculation of Inverse((Transpose(A) * A) */
  status = arm_mat_inverse_f32(&ATMA, &ATMAI);

  /* calculation of (Inverse((Transpose(A) * A)) *  Transpose(A)) */
  status = arm_mat_mult_f32(&ATMAI, &AT, &ATMA);

  /* Initialise B Matrix Instance with numRows, numCols and data array(B_f32) */
  srcRows = 4;
  srcColumns = 1;
  arm_mat_init_f32(&B, srcRows, srcColumns, (float32_t *)B_f32);

  /* Initialise X Matrix Instance with numRows, numCols and data array(X_f32) */
  srcRows = 4;
  srcColumns = 1;
  arm_mat_init_f32(&X, srcRows, srcColumns, X_f32);

  /* calculation ((Inverse((Transpose(A) * A)) *  Transpose(A)) * B) */
  status = arm_mat_mult_f32(&ATMA, &B, &X);

  /* Comparison of reference with test output */
  snr = arm_snr_f32((float32_t *)xRef_f32, X_f32, 4);

  /*------------------------------------------------------------------------------
  *            Initialise status depending on SNR calculations
  *------------------------------------------------------------------------------*/
  if( snr > SNR_THRESHOLD)
  {
    status = ARM_MATH_SUCCESS;
  }
  else
  {
    status = ARM_MATH_TEST_FAILURE;
  }


  /* ----------------------------------------------------------------------
  ** Loop here if the signals fail the PASS check.
  ** This denotes a test failure
  ** ------------------------------------------------------------------- */
  if( status != ARM_MATH_SUCCESS)
  {
    while(1);
  }

  while(1);                             /* main function does not return */
}
Beispiel #6
0
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
}
Beispiel #7
0
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);
}
Beispiel #8
0
void srcdkfTimeUpdateReduced(srcdkf_t *f, float32_t *u, float32_t dt, bool updateBias) {
	int S = f->S;			// number of states
	int V;			// number of noise variables
	int L;				// number of sigma points
	float32_t *x = f->x.pData;	// state estimate
	arm_matrix_instance_f32 *Sv;				// process covariance
	float32_t *Xa = f->Xa.pData;	// augmented sigma points
	float32_t *xIn = f->xIn;	// callback buffer
	float32_t *xOut = f->xOut;	// callback buffer
	float32_t *xNoise = f->xNoise;	// callback buffer
	float32_t *qrTempS = f->qrTempS.pData;
	int i=0, j=0, k=0;

	V = f->V;
	Sv = &f->Sv;
	/*
	//if(!updateBias) {
		printf("II: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f 11:%8.4f 12:%8.4f 13:%8.4f 14:%8.4f\n",
				x[0],x[1],x[2],x[3],x[4],
				x[5],x[6],x[7],x[8],x[9],
				x[10],x[11],x[12],x[13]);
		usleep(100);
	//}*/

	srcdkfCalcSigmaPoints(f, Sv);
	L = f->L;

	// Xa = f(Xx, Xv, u, dt)
	for (i = 0; i < L; i++) {
		// i is column
		// Create input values
		for (j = 0; j < S; j++)
			// j is row
			xIn[j] = Xa[j*L + i];

		for (j = 0; j < V; j++)
			xNoise[j] = Xa[(S+j)*L + i];

		// Do time update
		if(updateBias) {
			f->timeUpdate(xIn, xNoise, xOut, u, dt, updateBias);
		} else {
			if (i == 0) {
				// do time update and remember result
				f->timeUpdate(xIn, xNoise, xOut, u, dt, updateBias);
				for (k = 0; k < S; k++) {
					f->SxTemp.pData[k] = xOut[k];
				}
			} else if (i < 1 + S + 9) {
				// this is a nonlinear problem do the calculations
				f->timeUpdate(xIn, xNoise, xOut, u, dt, updateBias);
			} else if (i < 1 + S + V) {
				// linear problem its not needed to do each calculation
				// because Sv is only trace matrix and medium is mean value
				for (k = 0; k < S; k++) {
					xOut[k] = f->SxTemp.pData[k];
				}
			} else if (i < 1 + S + V + S + 9) {
				// this is a nonlinear problem do the calculations
				f->timeUpdate(xIn, xNoise, xOut, u, dt, updateBias);
			} else if (i < 1 + S + V + S + V) {
				// linear problem its not needed to do each calculation
				for (k = 0; k < S; k++) {
					xOut[k] = f->SxTemp.pData[k];
				}
			}
		}

		// Save result to the augmented states
		for (j = 0; j < S; j++) {
			Xa[j*L + i] = xOut[j];
		}
/*
		if(startPosition > 0) {
			printf("xIn: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f 11:%8.4f 12:%8.4f 13:%8.4f 14:%8.4f\n",
					xIn[0],xIn[1],xIn[2],xIn[3],xIn[4],
					xIn[5],xIn[6],xIn[7],xIn[8],xIn[9],
					xIn[10],xIn[11],xIn[12],xIn[13]);
			usleep(200);
		}
		if(startPosition > 0) {
			printf("xNo: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f\n",
					xNoise[0],xNoise[1],xNoise[2],xNoise[3],xNoise[4],
					xNoise[5],xNoise[6],xNoise[7],xNoise[8],xNoise[9]);
			usleep(200);
		}
		if(startPosition > 0) {
			printf("xOu: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f 11:%8.4f 12:%8.4f 13:%8.4f 14:%8.4f\n",
					xOut[0],xOut[1],xOut[2],xOut[3],xOut[4],
					xOut[5],xOut[6],xOut[7],xOut[8],xOut[9],
					xOut[10],xOut[11],xOut[12],xOut[13]);
			usleep(200);
		}*/
	}

	// sum weighted resultant sigma points to create estimated state
	f->w0m = (f->hh - (float32_t)(S+V)) / f->hh;
	for (i = 0; i < S; i++) {
		int rOffset = i*L;
		x[i] = Xa[rOffset + 0] * f->w0m;
		for (j = 1; j < L; j++) {
			x[i] += Xa[rOffset + j] * f->wim;
		}
	}
/*
	//if(!updateBias) {
		printf("OO: 1:%8.4f 2:%8.4f 3:%8.4f 4:%8.4f 5:%8.4f 6:%8.4f 7:%8.4f 8:%8.4f 9:%8.4f 10:%8.4f 11:%8.4f 12:%8.4f 13:%8.4f 14:%8.4f\n",
				x[0],x[1],x[2],x[3],x[4],
				x[5],x[6],x[7],x[8],x[9],
				x[10],x[11],x[12],x[13]);
		usleep(200);
	//}*/

	// update state covariance
	for (i = 0; i < S; i++) {
		int rOffset = i*(S+V)*2;
		for (j = 0; j < S+V; j++) {
			qrTempS[rOffset + j] = (Xa[i*L + j + 1] - Xa[i*L + S+V + j + 1]) * f->wic1;
			qrTempS[rOffset + S+V + j] = (Xa[i*L + j + 1] + Xa[i*L + S+V + j + 1] - 2.0f*Xa[i*L + 0]) * f->wic2;
		}
	}
//matrixDump("qrTempS", &f->qrTempS);
//yield(1);
	qrDecompositionT_f32(&f->qrTempS, NULL, &f->SxT);   // with transposition
//matrixDump("SxT", &f->SxT);
//yield(200);
	arm_mat_trans_f32(&f->SxT, &f->Sx);

	/*
	usleep(1000000);
	printf("++++++++++++++++++++++++++++++++++++++++++++\n\n\n\n\n");
	for(i = 0; i < f->Sx.numRows; i++) {
		for (j = 0; f->Sx.numCols; j++ ) {
			printf("%8.4f ",f->Sx.pData[i*f->Sx.numCols + j]);
		}
		printf("\n");
		usleep(20000);
	}
	printf("++++++++++++++++++++++++++++++++++++++++++++\n\n\n\n\n");
	usleep(1000000);*/
}
void kalman_filter(kalman_filter_state *buffer_filtro, float medida_gyro[], float medida_accel[], float medida_mag[], float angles[], uint16_t estado_motores)
{
    GPIO_ResetBits(GPIOD, GPIO_Pin_14);
    //Insf_tancias das matrizes utilizadas para o cálculo
    arm_matrix_instance_f32 X;			//Matriz de estados. [n,1]
    arm_matrix_instance_f32 F;			//Matriz de transição de estados. [n,n]
    arm_matrix_instance_f32 Ft;			//Matriz de transição de estados transposta. [n,n]
    arm_matrix_instance_f32 I;			//Matriz identidadee. [n,n]
    arm_matrix_instance_f32 P;			//Matriz de confiabilidade do processo de atualização. [n,n]
    //arm_matrix_instance_f32 h;		//Matriz de mapeamento de observabilidade [a,n]
    arm_matrix_instance_f32 H;			//Matriz Jacobiana para atualização da confiabilidade do erro [a, n].
    arm_matrix_instance_f32 Ht;			//Matriz Jacobiana transposta para atualização da confiabilidade do erro. [n, a]
    arm_matrix_instance_f32 Q;			//Matriz de covariância multiplicada de processos; [n, n]
    arm_matrix_instance_f32 R;			//Matriz de variância [a ,a]
    arm_matrix_instance_f32 S;			//Matriz .... [a, a]
    arm_matrix_instance_f32 Sinv;		//Matriz S inversa. [a, a]
    arm_matrix_instance_f32 K;			//Matriz com os ganhos de Kalman [n,a]

    //Matrices intermediàrias para cálculo

    arm_matrix_instance_f32 temp_calc_a1_0;
    arm_matrix_instance_f32 temp_calc_a1_1;

    arm_matrix_instance_f32 temp_calc_n1_0;
    arm_matrix_instance_f32 temp_calc_n1_1;
	
    arm_matrix_instance_f32 temp_calc_aa_0;
    arm_matrix_instance_f32 temp_calc_aa_1;
	
    arm_matrix_instance_f32 temp_calc_na_0;
    arm_matrix_instance_f32 temp_calc_an_0;

    arm_matrix_instance_f32 temp_calc_nn_0;
    arm_matrix_instance_f32 temp_calc_nn_1;

	//Matriz S...
    float S_f32[a*a];
    arm_mat_init_f32(&S, a, a, S_f32);

    float Sinv_f32[a*a];
    arm_mat_init_f32(&Sinv, a, a, Sinv_f32);

	//Matriz do ganho de Kalman 
    float K_f32[n*a];
    arm_mat_init_f32(&K, n, a, K_f32);

	//Matrizes de suporte para o cálculo
		//Matrizes de 9 linhas e 1 coluna
    float temp_calc_n1_0_f32[n];
    float temp_calc_n1_1_f32[n];
	
    arm_mat_init_f32(&temp_calc_n1_0, n, 1, temp_calc_n1_0_f32);
    arm_mat_init_f32(&temp_calc_n1_1, n, 1, temp_calc_n1_1_f32);

	//Matrizes de 9 linhas e 1 coluna
    float temp_calc_a1_0_f32[a];
    float temp_calc_a1_1_f32[a];
	
    arm_mat_init_f32(&temp_calc_a1_0, a, 1, temp_calc_a1_0_f32);
    arm_mat_init_f32(&temp_calc_a1_1, a, 1, temp_calc_a1_1_f32);

	//Matrizes de 6 linhas e 6 colunas
    float temp_calc_aa_0_f32[a*a];
    float temp_calc_aa_1_f32[a*a];

    arm_mat_init_f32(&temp_calc_aa_0, a, a, temp_calc_aa_0_f32);
    arm_mat_init_f32(&temp_calc_aa_1, a, a, temp_calc_aa_1_f32);
	
	//Matrizes de 9 linhas e 6 colunas
    float temp_calc_na_0_f32[n*a];

    arm_mat_init_f32(&temp_calc_na_0, n, a, temp_calc_na_0_f32);

	//Matrizes de 6 linhas e 9 colunas
    float temp_calc_an_0_f32[a*n];

    arm_mat_init_f32(&temp_calc_an_0, a, n, temp_calc_an_0_f32);

	//Matrizes de 9 linhas e 9 colunas
    float temp_calc_nn_0_f32[n*n];
    float temp_calc_nn_1_f32[n*n];

    arm_mat_init_f32(&temp_calc_nn_0, n, n, temp_calc_nn_0_f32);
    arm_mat_init_f32(&temp_calc_nn_1, n, n, temp_calc_nn_1_f32);
	
	/*************************************Atualização dos dados para cálcuo*******************************/
	//Variáveis para cálculos
	
	float dt = buffer_filtro->dt;

	/*Velocidades angulares subtraídas dos bias. */
    float p = medida_gyro[0];
    float q = medida_gyro[1];
    float r = medida_gyro[2];

	/*Atualização dos estados dos ângulos com base nas velocidades angulares e do bias estimado anteriormente*/
    float X_f32[n];
    arm_mat_init_f32(&X, n, 1, X_f32);

    arm_copy_f32(buffer_filtro->ultimo_estado, X_f32, n);

    float phi =  X_f32[0];
    float theta = X_f32[1];
    float psi =  X_f32[2];

    float bPhi = X_f32[9];
    float bTheta = X_f32[10];
    float bPsi = X_f32[11];

    //Atualizar o estado anterior - Apenas os ângulos precisam serm inicializados, uma vez que os bias são atualizados por uma identidade.
    X_f32[0] = phi + (p)*dt + f_sin(phi)*f_tan(theta)*(q)*dt + f_cos(phi)*f_tan(theta)*(r)*dt;
    X_f32[1] = theta + f_cos(phi)*(q)*dt - f_sin(phi)*(r)*dt;
    X_f32[2] = psi + f_sin(phi)*f_sec(theta)*(q)*dt + f_cos(phi)*f_sec(theta)*(r)*dt;

    phi = X_f32[0];
    theta = X_f32[1];
    psi = X_f32[2];

    //Com os angulos calculados, cálcula os senos e cossenos utilizados para agilizar os processos de cálculo.
    float cos_Phi = f_cos(phi);
    float sin_Phi = f_sin(phi);

    float cos_Theta = f_cos(theta);
    float sin_Theta = f_sin(theta);
    float tan_Theta = f_tan(theta);

    //Elementos da matriz para atualização dos estados (F).
    float F_f32[n*n] = { dt*q*cos_Phi*tan_Theta - dt*r*sin_Phi*tan_Theta + 1,               dt*r*cos_Phi*(pow(tan_Theta,2) + 1) + dt*q*sin_Phi*(pow(tan_Theta,2) + 1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                                 - dt*r*cos_Phi - dt*q*sin_Phi,                                                                                 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                         (dt*q*cos_Phi)/cos_Theta - (dt*r*sin_Phi)/cos_Theta, (dt*r*cos_Phi*sin_Theta)/pow(cos_Theta,2) + (dt*q*sin_Phi*sin_Theta)/pow(cos_Theta,2), 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};

    arm_mat_init_f32(&F, n, n, F_f32);

	//Matriz Jacobiana transposta para atualização de P.
    float Ft_f32[n*n];
    arm_mat_init_f32(&Ft, n, n, Ft_f32);
    arm_mat_trans_f32(&F, &Ft);

	//Processo à priori para atualização da matriz de confiabilidade P.

	//Matriz de covariâncias do processo de atualização (Q).
    float qAngles = (buffer_filtro->Q_angles);
    float qBiasAcel = (buffer_filtro->Q_bias_acel);
    float qBiasMag = (buffer_filtro->Q_bias_mag);
    float qBiasAngles = (buffer_filtro->Q_bias_angle);

    /*Matriz de confiabilidade do processo de atualização. */
    /* Pk|k-1 = Pk-1|k-1 */
    float P_f32[n*n];
    arm_copy_f32(buffer_filtro->P, P_f32, n*n);
    arm_mat_init_f32(&P, n, n, P_f32);

    //temp_calc_nn_0 = F*P
    if(arm_mat_mult_f32(&F, &P, &temp_calc_nn_0) != ARM_MATH_SUCCESS)
        while(1);

    //temp_calc_nn_1 = F*P*F'
    if(arm_mat_mult_f32(&temp_calc_nn_0, &Ft, &temp_calc_nn_1) != ARM_MATH_SUCCESS)
        while(1);


    //Atualiza a matriz de covariâncias dos processos
    float Q_f32[n*n] = {	qAngles,   0,           0,          0,          0,          0,          0,          0,          0,          0,          0,              0,
                            0,          qAngles,    0,          0,          0,          0,          0,          0,          0,          0,          0,              0,
                            0,          0,          qAngles,	0,          0,          0,          0,          0,          0,          0,          0,              0,
                            0,          0,          0,          qBiasAcel,  0,          0,          0,          0,          0,          0,          0,              0,
                            0,          0,          0,          0,          qBiasAcel,  0,          0,          0,          0,          0,          0,              0,
                            0,          0,      	0,          0,          0,          qBiasAcel,  0,          0,          0,          0,          0,              0,
                            0,          0,          0,          0,          0,          0,          qBiasMag,   0,          0,          0,          0,              0,
                            0,          0,          0,          0,          0,          0,          0,          qBiasMag,   0,          0,          0,              0,
                            0,          0,          0,          0,          0,          0,          0,          0,          qBiasMag,   0,          0,              0,
                            0,          0,          0,          0,          0,          0,          0,          0,          0,          qBiasAngles, 0,             0,
                            0,          0,          0,          0,          0,          0,          0,          0,          0,          0,          qBiasAngles,    0,
                            0,          0,          0,          0,          0,          0,          0,          0,          0,          0,          0,              qBiasAngles};

    arm_mat_init_f32(&Q, n, n, Q_f32);

    //P = temp_calc_nn_1 + Q = F*P*F' + Q
    if(arm_mat_add_f32(&temp_calc_nn_1, &Q, &P) != ARM_MATH_SUCCESS)
        while(1);

	/*Estados iniciais do magnetômetro */
    float magRefVector[3];
    float acelRefVector[3];
    arm_matrix_instance_f32 magRefMatrix;
    arm_matrix_instance_f32 acelRefMatrix;

    arm_mat_init_f32(&magRefMatrix, 3, 1, magRefVector);
    arm_mat_init_f32(&acelRefMatrix, 3, 1, acelRefVector);

    float ax = buffer_filtro->AcelInicial[0];
    float ay = buffer_filtro->AcelInicial[1];
    float az = buffer_filtro->AcelInicial[2];

    float hx = buffer_filtro->MagInicial[0];
    float hy = buffer_filtro->MagInicial[1];
    float hz = buffer_filtro->MagInicial[2];

    magRefVector[0] = hx;
    magRefVector[1] = hy;
    magRefVector[2] = hz;

    acelRefVector[0] = ax;
    acelRefVector[1] = ay;
    acelRefVector[2] = az;

    //Matrizes com o resultado das operações de rotação
    float observatedStateVector[a];

    arm_matrix_instance_f32 observatedStateMatrix;
    arm_mat_init_f32(&observatedStateMatrix, a, 1, observatedStateVector);

    //Matriz contendo os valores observados utilizados pra gerar o rezíduo (yk)
    arm_matrix_instance_f32 rotatedMagMatrix;
    arm_matrix_instance_f32 rotatedAcelMatrix;

    arm_mat_init_f32(&rotatedAcelMatrix, 3, 1, observatedStateVector);
    arm_mat_init_f32(&rotatedMagMatrix, 3, 1, observatedStateVector+3);

    //Matriz de rotação com base nos angulos estimados.
    float rotationVector[9];
    arm_matrix_instance_f32 rotationMatrix;
    arm_mat_init_f32(&rotationMatrix, 3, 3, rotationVector);
    getABCRotMatFromEulerAngles(phi-bPhi, theta-bTheta, psi-bPsi, &rotationMatrix);

    /* Cálculo das referências com base no magnetômetro e no estado do acelerômetro parado [0; 0; 1]; */
    if(arm_mat_mult_f32(&rotationMatrix, &acelRefMatrix, &rotatedAcelMatrix) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_mult_f32(&rotationMatrix, &magRefMatrix, &rotatedMagMatrix) != ARM_MATH_SUCCESS)
        while(1);

    //Vetor com as médidas
    float zkVector[a];

    zkVector[0] = medida_accel[0];
    zkVector[1] = medida_accel[1];
    zkVector[2] = medida_accel[2];

    zkVector[3] = medida_mag[0];
    zkVector[4] = medida_mag[1];
    zkVector[5] = medida_mag[2];

    zkVector[6] = angles[0];
    zkVector[7] = angles[1];
    zkVector[8] = angles[2];

    arm_matrix_instance_f32 zkMatrix;
    arm_mat_init_f32(&zkMatrix, a, 1, zkVector);

    //Vetor de resíduo
    float ykVector[a];
    arm_matrix_instance_f32 ykMatrix;
    arm_mat_init_f32(&ykMatrix, a, 1, ykVector);

    //Adiciona os bias estimados pelo filtro
    observatedStateVector[0] += X_f32[3];
    observatedStateVector[1] += X_f32[4];
    observatedStateVector[2] += X_f32[5];
    observatedStateVector[3] += X_f32[6];
    observatedStateVector[4] += X_f32[7];
    observatedStateVector[5] += X_f32[8];

    //Remove o bias estimado pelo filtro
    float correctedPhi =    -(X_f32[0] - X_f32[9]);
    float correctedTheta =  -(X_f32[1] - X_f32[10]);
    float correctedPsi =    -(X_f32[2] - X_f32[11]);

    //Com os angulos corrigidos calculados, cálcula os senos e cossenos utilizados para agilizar os processos de cálculo.
    float cos_correctedPhi = f_cos(correctedPhi);
    float sin_correctedPhi = f_sin(correctedPhi);

    float cos_correctedTheta = f_cos(correctedTheta);
    float sin_correctedTheta = f_sin(correctedTheta);

    float cos_correctedPsi = f_cos(correctedPsi);
    float sin_correctedPsi = f_sin(correctedPsi);
    //
    observatedStateVector[6] = -correctedPhi;
    observatedStateVector[7] = -correctedTheta;
    observatedStateVector[8] = -correctedPsi;

    if(arm_mat_sub_f32(&zkMatrix, &observatedStateMatrix, &ykMatrix) != ARM_MATH_SUCCESS)
        while(1);

    float H_f32[a*n] = {                                                                                                                                                                                                                                0,                                                 ax*sin_correctedTheta*cos_correctedPsi - az*cos_correctedTheta - ay*sin_correctedPsi*sin_correctedTheta,                                                                                                         ay*cos_correctedPsi*cos_correctedTheta + ax*sin_correctedPsi*cos_correctedTheta, 1, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                  0,                                                 az*cos_correctedTheta - ax*sin_correctedTheta*cos_correctedPsi + ay*sin_correctedPsi*sin_correctedTheta,                                                                                                       - ay*cos_correctedPsi*cos_correctedTheta - ax*sin_correctedPsi*cos_correctedTheta,
                         ax*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) + ay*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) + az*cos_correctedPhi*cos_correctedTheta, ay*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta - ax*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - az*sin_correctedPhi*sin_correctedTheta, ay*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) - ax*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta), 0, 1, 0, 0, 0, 0, - ax*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - ay*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - az*cos_correctedPhi*cos_correctedTheta, az*sin_correctedPhi*sin_correctedTheta + ax*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - ay*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta, ax*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - ay*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi),
                         az*sin_correctedPhi*cos_correctedTheta - ay*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - ax*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi), az*sin_correctedTheta*cos_correctedPhi + ax*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - ay*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta, ay*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - ax*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi), 0, 0, 1, 0, 0, 0,   ax*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) + ay*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - az*sin_correctedPhi*cos_correctedTheta, ay*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta - ax*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - az*sin_correctedTheta*cos_correctedPhi, ax*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - ay*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi),
                                                                                                                                                                                                                                                        0,                                                 hx*sin_correctedTheta*cos_correctedPsi - hz*cos_correctedTheta - hy*sin_correctedPsi*sin_correctedTheta,                                                                                                         hy*cos_correctedPsi*cos_correctedTheta + hx*sin_correctedPsi*cos_correctedTheta, 0, 0, 0, 1, 0, 0,                                                                                                                                                                                                                                  0,                                                 hz*cos_correctedTheta - hx*sin_correctedTheta*cos_correctedPsi + hy*sin_correctedPsi*sin_correctedTheta,                                                                                                       - hy*cos_correctedPsi*cos_correctedTheta - hx*sin_correctedPsi*cos_correctedTheta,
                         hx*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) + hy*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) + hz*cos_correctedPhi*cos_correctedTheta, hy*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta - hx*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - hz*sin_correctedPhi*sin_correctedTheta, hy*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) - hx*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta), 0, 0, 0, 0, 1, 0, - hx*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - hy*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - hz*cos_correctedPhi*cos_correctedTheta, hz*sin_correctedPhi*sin_correctedTheta + hx*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - hy*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta, hx*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - hy*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi),
                         hz*sin_correctedPhi*cos_correctedTheta - hy*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - hx*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi), hz*sin_correctedTheta*cos_correctedPhi + hx*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - hy*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta, hy*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - hx*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi), 0, 0, 0, 0, 0, 1,   hx*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) + hy*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - hz*sin_correctedPhi*cos_correctedTheta, hy*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta - hx*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - hz*sin_correctedTheta*cos_correctedPhi, hx*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - hy*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi),
                                                                                                                                                                                                                                                        1,                                                                                                                                                        0,                                                                                                                                                                                       0, 0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                 -1,                                                                                                                                                        0,                                                                                                                                                                                       0,
                                                                                                                                                                                                                                                        0,                                                                                                                                                        1,                                                                                                                                                                                       0, 0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                  0,                                                                                                                                                       -1,                                                                                                                                                                                       0,
                                                                                                                                                                                                                                                        0,                                                                                                                                                        0,                                                                                                                                                                                       1, 0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                  0,                                                                                                                                                        0,                                                                                                                                                                                      -1};

    arm_mat_init_f32(&H, a, n, H_f32);

	/* Matriz Jacobiana transposta para cálculo da confiabilidade do erro . */
    float Ht_f32[n*a];
    arm_mat_init_f32(&Ht, n, a, Ht_f32);

    arm_mat_trans_f32(&H, &Ht);

	//Matriz de variâncias
	float Racel = buffer_filtro->R_acel;
	float Rmag = buffer_filtro->R_mag;	 //Variância inicial do magnetômetro.
    float Rangles = buffer_filtro->R_angles;

    float acelModulus = getVectorModulus(medida_accel, 3);
//    float magModulus = getVectorModulus(medida_mag, 3);
//    float magInitialModulus = getVectorModulus(buffer_filtro->MagInicial, 3);

    //Racel += 100*fabsf(1 - acelModulus);
    //Rmag += 1*fabs(magInitialModulus - magModulus);


    float R_f32[a*a] = {(Racel), 0, 0, 0, 0, 0, 0, 0, 0,
                        0, (Racel), 0, 0, 0, 0, 0, 0, 0,
                        0, 0, (Racel), 0, 0, 0, 0, 0, 0,
                        0, 0, 0, (Rmag),  0, 0, 0, 0, 0,
                        0, 0, 0, 0, (Rmag),	 0, 0, 0, 0,
                        0, 0, 0, 0, 0, (Rmag), 0, 0, 0,
                        0, 0, 0, 0, 0, 0, (Rangles), 0, 0,
                        0, 0, 0, 0, 0, 0, 0, (Rangles), 0,
                        0, 0, 0, 0, 0, 0, 0, 0, (Rangles)};

    arm_mat_init_f32(&R, a, a, R_f32);

	//Cálculos do filtro de Kalman

	//S = H*P*H' + R
    if(arm_mat_mult_f32(&H, &P, &temp_calc_an_0) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_mult_f32(&temp_calc_an_0, &Ht, &temp_calc_aa_0) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_add_f32(&temp_calc_aa_0, &R, &S) != ARM_MATH_SUCCESS)
        while(1);

    //Sinv = inv(S);
    //if(arm_mat_inverse_f32(&S, &Sinv) == ARM_MATH_SINGULAR)
    //    while(1);
    arm_mat_inverse_f32(&S, &Sinv);

    //Kk = P*Ht*S^(-1)
		//P*Ht
    if(arm_mat_mult_f32(&P, &Ht, &temp_calc_na_0) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_mult_f32(&temp_calc_na_0, &Sinv, &K) != ARM_MATH_SUCCESS)
        while(1);
	
    //temp_calc_n11 = Kk*y
    if(arm_mat_mult_f32(&K, &ykMatrix, &temp_calc_n1_0) != ARM_MATH_SUCCESS)
        while(1);

    //X = X + temp_calc_n1_1;
    if(arm_mat_add_f32(&X, &temp_calc_n1_0, &temp_calc_n1_1) != ARM_MATH_SUCCESS)
        while(1);

    arm_copy_f32(temp_calc_n1_1_f32, X_f32, n);

	//P = (I-K*H)*P
	
	//Matriz identidade para atualização da matriz P à posteriori.
    float I_f32[n*n] = {	1,0,0,0,0,0,0,0,0,0,0,0,
                            0,1,0,0,0,0,0,0,0,0,0,0,
                            0,0,1,0,0,0,0,0,0,0,0,0,
                            0,0,0,1,0,0,0,0,0,0,0,0,
                            0,0,0,0,1,0,0,0,0,0,0,0,
                            0,0,0,0,0,1,0,0,0,0,0,0,
                            0,0,0,0,0,0,1,0,0,0,0,0,
                            0,0,0,0,0,0,0,1,0,0,0,0,
                            0,0,0,0,0,0,0,0,1,0,0,0,
                            0,0,0,0,0,0,0,0,0,1,0,0,
                            0,0,0,0,0,0,0,0,0,0,1,0,
                            0,0,0,0,0,0,0,0,0,0,0,1};

    arm_mat_init_f32(&I, n, n, I_f32);


    if(arm_mat_mult_f32(&K, &H, &temp_calc_nn_0) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_sub_f32(&I, &temp_calc_nn_0, &temp_calc_nn_1) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_mult_f32(&temp_calc_nn_1, &P, &temp_calc_nn_0) != ARM_MATH_SUCCESS)
        while(1);

    arm_copy_f32(X_f32, buffer_filtro->ultimo_estado, n);
    arm_copy_f32(temp_calc_nn_0_f32, buffer_filtro->P, n*n);
}
Beispiel #10
0
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 EKF_Attitude::innovate_priori(Quaternion& quaternion,
					float Gx,
					float Gy,
					float Gz){

	float quaternion_array[4] = {quaternion.w, quaternion.x, quaternion.y, quaternion.z};
	arm_matrix_instance_f32 q;
	arm_mat_init_f32(&q, 4, 1, quaternion_array);

	static uint32_t timestamp;
	static float 	dt;

	dt = Time.get_time_since_sec(timestamp);

	//---------------------- PREDICTION -----------------------------
	//1. Obtain the angular velocities:
	float wx = (Gx*2*PI)/360.0;
	float wy = (Gy*2*PI)/360.0;
	float wz = (Gz*2*PI)/360.0;

	//2. Calculate the discrete time state transition matrix:
	float32_t omega_f32[16] = {
			0, -wx, -wy, -wz,
			wx, 0,   wz, -wy,
			wy, -wz, 0,   wx,
			wz, wy, -wx,  0};

	arm_matrix_instance_f32 omega;      /* Matrix Omega Instance */
	arm_mat_init_f32(&omega, 4, 4, omega_f32);

	//Ak = I + 0.5*omega*T;
	arm_mat_scale_f32(&omega, 0.5*dt, &temp4x4);
	arm_mat_add_f32(&I, &temp4x4, &Ak);

	//3. Calculation of the a priori system state:
	// q = Ak*q
	arm_mat_mult_f32(&Ak, &q, &temp4x1);

	//memcpy((void*)quaternion, (void*)temp4x1_f32, 4);
	/*quaternion[0] = temp4x1_f32[0];
	quaternion[1] = temp4x1_f32[1];
	quaternion[2] = temp4x1_f32[2];
	quaternion[3] = temp4x1_f32[3];*/
	quaternion.w = temp4x1_f32[0];
	quaternion.x = temp4x1_f32[1];
	quaternion.y = temp4x1_f32[2];
	quaternion.z = temp4x1_f32[3];

	//4. Calculation of the a proiri noise covariance matrix:
	//   Pk = Ak*Pk*Ak.' + Qk;
	arm_mat_trans_f32(&Ak, &Ak_trans);

	//temp = Ak*Pk
	arm_mat_mult_f32(&Ak, &Pk, &temp4x4);
	//Pk = temp*Ak.'
	arm_mat_mult_f32(&temp4x4, &Ak_trans, &Pk);
	//temp = Pk + Qk:
	arm_mat_add_f32(&Pk, &Qk, &temp4x4);
	//Pk = temp;
	copy_matrix(&Pk, &temp4x4);

	timestamp = Time.get_timestamp();
}
void EKF_Attitude::innovate_stage2(Quaternion& quaternion,
							 float Mx,
							 float My,
							 float Mz){
	float quaternion_array[4] = {quaternion.w, quaternion.x, quaternion.y, quaternion.z};
	arm_matrix_instance_f32 q;
	arm_mat_init_f32(&q, 4, 1, quaternion_array);

	//----------------------   MEASUREMENT UPDATE   -----------------------------
	//1: Calculation of the Jacobian matrix:
		float32_t H_k2_f32[16] = {
				 2*quaternion.z,  2*quaternion.y,  2*quaternion.x,  2*quaternion.w,
				 2*quaternion.w, -2*quaternion.x, -2*quaternion.y, -2*quaternion.z,
				-2*quaternion.x, -2*quaternion.w,  2*quaternion.z,  2*quaternion.y
		};
		arm_matrix_instance_f32 H_k2;      /* Matrix Omega Instance */
		arm_mat_init_f32(&H_k2, 3, 4, H_k2_f32);

	//2: Calculate the Kalman Gain:
		/*    S = inv(H_k2 * Pk * H_k2.' + V_k2 * R_k2 * V_k2.');
			  K_k1 = Pk * H_k2.' * S;
		*/

		//temp3x3 = H_k2 * Pk * H_k2.'
		arm_mat_trans_f32(&H_k2, &temp4x3);
		arm_mat_mult_f32(&H_k2, &Pk, &temp3x4);
		arm_mat_mult_f32(&temp3x4, &temp4x3, &temp3x3);

		//temp3x3 = inv(temp3x3 + R)   ( = S!!)
		arm_mat_add_f32(&temp3x3, &R, &S);
		arm_mat_inverse_f32(&S, &temp3x3);

		//K_k2 = Pk * H_k2.'    (temp4x3 = H_k2.')
		arm_mat_mult_f32(&Pk, &temp4x3, &K_k2);
		arm_mat_mult_f32(&K_k2, &temp3x3, &temp4x3);

		copy_matrix(&K_k2, &temp4x3);

	//3: Reading of the magnetometer data:
		float32_t z_k2_f32[3] = {
				Mx,
				My,
				Mz,
		};
		arm_matrix_instance_f32 z_k2;      /* Matrix Omega Instance */
		arm_mat_init_f32(&z_k2, 3, 1, z_k2_f32);

   //4: Calculation of h2(q):
		float32_t h2_q_f32[3] = {
				2*quaternion.x*quaternion.y + 2*quaternion.w*quaternion.z,
				  quaternion.w*quaternion.w - quaternion.x*quaternion.x- quaternion.y*quaternion.y - quaternion.z*quaternion.z,
				2*quaternion.y*quaternion.z - 2*quaternion.w*quaternion.x
		};
		arm_matrix_instance_f32 h2_q;      /* Matrix Omega Instance */
		arm_mat_init_f32(&h2_q, 3, 1, h2_q_f32);

    //5: Calculation of the correction factor
    	//q_corr_2 = K_k2*(z_k2 - h2_q);
		arm_mat_sub_f32(&z_k2, &h2_q, &temp3x1);
		arm_mat_mult_f32(&K_k2, &temp3x1, &q_corr_2);
		q_corr_2.pData[1] = 0;
		q_corr_2.pData[2] = 0;

    //6: Calculation of the a posteriori state estimation
	    //q = q + q_2;
		arm_mat_add_f32(&q, &q_corr_2, &temp4x1);
		copy_matrix(&q, &temp4x1);

	//7: Calculation of a aposteriori error covariance matrix
	    //Pk = (I - K_k2 * H_k2)*Pk;
		arm_mat_mult_f32(&K_k2, &H_k2, &temp4x4);
		arm_mat_sub_f32(&I, &temp4x4, &temp4x4_2);

		arm_mat_mult_f32(&temp4x4_2, &Pk, &temp4x4);
		copy_matrix(&Pk, &temp4x4);

		quaternion.w = quaternion_array[0];
		quaternion.x = quaternion_array[1];
		quaternion.y = quaternion_array[2];
		quaternion.z = quaternion_array[3];
}
void EKF_Attitude::innovate_stage1(Quaternion& quaternion,
							 float Ax,
							 float Ay,
							 float Az){
	float quaternion_array[4] = {quaternion.w, quaternion.x, quaternion.y, quaternion.z};
	arm_matrix_instance_f32 q;
	arm_mat_init_f32(&q, 4, 1, quaternion_array);

	//----------------------   MEASUREMENT UPDATE   -----------------------------
	//1: Calculation of the Jacobian matrix:
		float32_t H_k1_f32[16] = {
				-2*quaternion.y,  2*quaternion.z, -2*quaternion.w, 2*quaternion.x,
				 2*quaternion.x,  2*quaternion.w,  2*quaternion.z, 2*quaternion.y,
				 2*quaternion.w, -2*quaternion.x, -2*quaternion.y, 2*quaternion.z
		};
		arm_matrix_instance_f32 H_k1;      /* Matrix Omega Instance */
		arm_mat_init_f32(&H_k1, 3, 4, H_k1_f32);

	//2: Calculate the Kalman Gain:
		/*    S = inv(H_k1 * Pk * H_k1.' + V_k1*R_k1*V_k1.');
			  K_k1 = Pk * H_k1.' * S;
		*/

		//temp3x3 = H_k1 * Pk * H_k1.'
		arm_mat_trans_f32(&H_k1, &temp4x3);
		arm_mat_mult_f32(&H_k1, &Pk, &temp3x4);
		arm_mat_mult_f32(&temp3x4, &temp4x3, &temp3x3);

		//temp3x3 = inv(temp3x3 + R)   ( = S!!)
		arm_mat_add_f32(&temp3x3, &R, &S);
		arm_mat_inverse_f32(&S, &temp3x3);

		//K_k1 = Pk * H_k1.'    (temp4x3 = H_k1.')
		arm_mat_mult_f32(&Pk, &temp4x3, &K_k1);
		arm_mat_mult_f32(&K_k1, &temp3x3, &temp4x3);

		copy_matrix(&K_k1, &temp4x3);

		//3: Reading of the accelerometer data:
		float32_t z_f32[3] = {
				Ax,
				Ay,
				Az,
		};
		arm_matrix_instance_f32 z;      /* Matrix Omega Instance */
		arm_mat_init_f32(&z, 3, 1, z_f32);

   //4: Calculation of h1(q):
		float32_t h1_q_f32[3] = {
				2*quaternion.x*quaternion.z-2*quaternion.w*quaternion.y,
				2*quaternion.w*quaternion.x+2*quaternion.y*quaternion.z,
				quaternion.w*quaternion.w - quaternion.x*quaternion.x- quaternion.y*quaternion.y + quaternion.z*quaternion.z
		};
		arm_matrix_instance_f32 h1_q;      /* Matrix Omega Instance */
		arm_mat_init_f32(&h1_q, 3, 1, h1_q_f32);

	//5: Calculation of the correction factor
		//q_corr = K_k1*(z - h1_q);
		arm_mat_sub_f32(&z, &h1_q, &temp3x1);
		arm_mat_mult_f32(&K_k1, &temp3x1, &q_corr);
		q_corr.pData[3] = 0;

	//6: Calculation of the a posteriori state estimation
		//q = q + q_1;
		arm_mat_add_f32(&q, &q_corr, &temp4x1);
		copy_matrix(&q, &temp4x1);

	//7: Calculation of a aposteriori error covariance matrix
		//Pk = (I - K_k1 * H_k1)*Pk;
		arm_mat_mult_f32(&K_k1, &H_k1, &temp4x4);
		arm_mat_sub_f32(&I, &temp4x4, &temp4x4_2);

		arm_mat_mult_f32(&temp4x4_2, &Pk, &temp4x4);
		copy_matrix(&Pk, &temp4x4);

		quaternion.w = quaternion_array[0];
		quaternion.x = quaternion_array[1];
		quaternion.y = quaternion_array[2];
		quaternion.z = quaternion_array[3];
}
Beispiel #14
0
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);
}