static void UKF_GenerateSigmaPoints(UKF_Filter* ukf) { int cols = 0; //state float32_t *X = ukf->X_f32; float32_t *tmpX = ukf->tmpX_f32; //need to tune!!! //covariance //c*chol(P)' arm_mat_chol_f32(&ukf->P); arm_mat_scale_f32(&ukf->P, ukf->gamma, &ukf->P); ////////////////////////////////////////////////////////////////////////// //generate sigma points arm_mat_setcolumn_f32(&ukf->XSP, X, cols); for(cols = 1; cols <= UKF_STATE_DIM; cols++){ arm_mat_getcolumn_f32(&ukf->P, tmpX, cols - 1); arm_mat_add_f32(&ukf->X, &ukf->tmpX, &ukf->tmpX); arm_mat_setcolumn_f32(&ukf->XSP, tmpX, cols); } for(cols = UKF_STATE_DIM + 1; cols < UKF_SP_POINTS; cols++){ arm_mat_getcolumn_f32(&ukf->P, tmpX, cols - 8/*UKF_STATE_DIM + 1*/); arm_mat_sub_f32(&ukf->X, &ukf->tmpX, &ukf->tmpX); arm_mat_setcolumn_f32(&ukf->XSP, tmpX, cols); } }
void Mat_scale ( ) { uint32_t R1 = 0; uint32_t C1 = 0; uint32_t R2 = 0; uint32_t C2 = 0; uint32_t Scaling_factor = 0; arm_status status; arm_matrix_instance_f32 A ; arm_matrix_instance_f32 Scale_A__OUT ; const float32_t A_data[4]; const float32_t Scale_A__OUT_data[4]; arm_mat_init_f32( &A , R1 , C1, A_data ); arm_mat_init_f32( &Scale_A__OUT , C2 , Scaling_factor , Scale_A__OUT_data ); status = arm_mat_scale_f32( &A , R2, &Scale_A__OUT ); }
EKF_Attitude::EKF_Attitude(){ //Parameters arm_mat_init_f32(&Pk, 4, 4, Pk_f32); arm_mat_init_f32(&R, 3, 3, R_f32); arm_mat_init_f32(&Qk, 4, 4, Qk_f32); arm_mat_init_f32(&I, 4, 4, I_f32); arm_mat_init_f32(&x, 2, 1, x_f32); //Helpers: arm_mat_init_f32(&Ak, 4, 4, Ak_f32); arm_mat_init_f32(&Ak_trans, 4, 4, Ak_trans_f32); arm_mat_init_f32(&S, 3, 3, S_f32); arm_mat_init_f32(&K_k1, 4, 3, K_k1_f32); arm_mat_init_f32(&K_k2, 4, 3, K_k2_f32); arm_mat_init_f32(&h1_q, 3, 1, h1_q_f32); arm_mat_init_f32(&q_corr, 4, 1, q_corr_f32); arm_mat_init_f32(&q_corr_2, 4, 1, q_corr_2_f32); //Temps arm_mat_init_f32(&temp4x4, 4, 4, temp4x4_f32); arm_mat_init_f32(&temp4x4_2, 4, 4, temp4x4_2_f32); arm_mat_init_f32(&temp3x4, 3, 4, temp3x4_f32); arm_mat_init_f32(&temp4x3, 4, 3, temp4x3_f32); arm_mat_init_f32(&temp3x3, 3, 3, temp3x3_f32); arm_mat_init_f32(&temp3x1, 3, 1, temp3x1_f32); arm_mat_init_f32(&temp4x1, 4, 1, temp4x1_f32); arm_mat_init_f32(&null4x4, 4, 4, null4x4_f32); //arm_mat_scale_f32(&R, 0.0001, &temp3x3); //copy_matrix(&R, &temp3x3); arm_mat_scale_f32(&Qk, 0.0001, &temp4x4); copy_matrix(&Qk, &temp4x4); }
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 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); }