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);
}
}
arm_matrix_instance_f32 c_rc_LQR2_errorStateVector(pv_type_datapr_attitude attitude,
pv_type_datapr_attitude attitude_reference,
pv_type_datapr_position position,
pv_type_datapr_position position_reference,
pv_type_datapr_servos servo,
pv_type_datapr_servos servo_reference){
arm_matrix_instance_f32 c_rc_LQR2_error_state_vector, c_rc_LQR2_state_vector, c_rc_LQR2_reference_state_vector;
//State Vector
c_rc_LQR2_state_vector_f32[STATE_X]=position.x;
c_rc_LQR2_state_vector_f32[STATE_Y]=position.y;
c_rc_LQR2_state_vector_f32[STATE_Z]=position.z;
c_rc_LQR2_state_vector_f32[STATE_ROLL]=attitude.roll;
c_rc_LQR2_state_vector_f32[STATE_PITCH]=attitude.pitch;
c_rc_LQR2_state_vector_f32[STATE_YAW]=attitude.yaw;
c_rc_LQR2_state_vector_f32[STATE_ALPHA_R]=servo.alphar;
c_rc_LQR2_state_vector_f32[STATE_ALPHA_L]=servo.alphal;
c_rc_LQR2_state_vector_f32[STATE_DX]=position.dotX;
c_rc_LQR2_state_vector_f32[STATE_DY]=position.dotY;
c_rc_LQR2_state_vector_f32[STATE_DZ]=position.dotZ;
c_rc_LQR2_state_vector_f32[STATE_DROLL]=attitude.dotRoll;
c_rc_LQR2_state_vector_f32[STATE_DPITCH]=attitude.dotPitch;
c_rc_LQR2_state_vector_f32[STATE_DYAW]=attitude.dotYaw;
c_rc_LQR2_state_vector_f32[STATE_DALPHA_R]=servo.dotAlphar;
c_rc_LQR2_state_vector_f32[STATE_DALPHA_L]=servo.dotAlphal;
//Updates the height equilibrium point according to the reference
c_rc_LQR2_state_vector_reference_f32[STATE_X]=position_reference.x;
c_rc_LQR2_state_vector_reference_f32[STATE_Y]=position_reference.y;
c_rc_LQR2_state_vector_reference_f32[STATE_Z]=position_reference.z;
c_rc_LQR2_state_vector_reference_f32[STATE_ROLL]=attitude_reference.roll;
c_rc_LQR2_state_vector_reference_f32[STATE_PITCH]=attitude_reference.pitch;
c_rc_LQR2_state_vector_reference_f32[STATE_YAW]=attitude_reference.yaw;
c_rc_LQR2_state_vector_reference_f32[STATE_ALPHA_R]=servo_reference.alphar;
c_rc_LQR2_state_vector_reference_f32[STATE_ALPHA_L]=servo_reference.alphal;
c_rc_LQR2_state_vector_reference_f32[STATE_DX]=position_reference.dotX;
c_rc_LQR2_state_vector_reference_f32[STATE_DY]=position_reference.dotY;
c_rc_LQR2_state_vector_reference_f32[STATE_DZ]=position_reference.dotZ;
c_rc_LQR2_state_vector_reference_f32[STATE_DROLL]=attitude_reference.dotRoll;
c_rc_LQR2_state_vector_reference_f32[STATE_DPITCH]=attitude_reference.dotPitch;
c_rc_LQR2_state_vector_reference_f32[STATE_DYAW]=attitude_reference.dotYaw;
c_rc_LQR2_state_vector_reference_f32[STATE_DALPHA_R]=servo_reference.dotAlphar;
c_rc_LQR2_state_vector_reference_f32[STATE_DALPHA_L]=servo_reference.dotAlphal;
//Initializes the matrices
arm_mat_init_f32(&c_rc_LQR2_state_vector, 16, 1, (float32_t *)c_rc_LQR2_state_vector_f32);
arm_mat_init_f32(&c_rc_LQR2_reference_state_vector, 16, 1, (float32_t *)c_rc_LQR2_state_vector_reference_f32);
arm_mat_init_f32(&c_rc_LQR2_error_state_vector, 16, 1, (float32_t *)c_rc_LQR2_error_state_vector_f32);
//e(t)=x(k)- xr(k)
arm_mat_sub_f32(&c_rc_LQR2_state_vector, &c_rc_LQR2_reference_state_vector, &c_rc_LQR2_error_state_vector);
return c_rc_LQR2_error_state_vector;
}
void Matrix_subtraction ( ) {
uint32_t r1 = 0;
uint32_t c1 = 0;
uint32_t cout = 0;
uint32_t rout = 0;
uint32_t c2 = 0;
uint32_t r2 = 0;
arm_status status;
arm_matrix_instance_f32 A ;
arm_matrix_instance_f32 B ;
arm_matrix_instance_f32 A_SUB_B_OUT ;
const float32_t A_data[4];
const float32_t B_data[4];
const float32_t A_SUB_B_OUT_data[4];
arm_mat_init_f32( &A ,
r1 ,
c1,
A_data );
arm_mat_init_f32( &B ,
r2 ,
c2,
B_data );
arm_mat_init_f32( &A_SUB_B_OUT ,
rout ,
cout ,
A_SUB_B_OUT_data );
status = arm_mat_sub_f32( &A ,
&B , &A_SUB_B_OUT );
}
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 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);
}
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
}
arm_matrix_instance_f32 c_rc_LQR2_errorStateVector_I(pv_type_datapr_attitude attitude,
pv_type_datapr_attitude attitude_reference,
pv_type_datapr_position position,
pv_type_datapr_position position_reference,
pv_type_datapr_servos servo,
pv_type_datapr_servos servo_reference,
float sample_time, bool stop){
arm_matrix_instance_f32 c_rc_LQR2_error_state_vector, c_rc_LQR2_state_vector, c_rc_LQR2_reference_state_vector;
//Integradores
//Vector integration of error(Trapezoidal method)
if (stop){
c_rc_LQR2_deltaxsi[0]=0;
c_rc_LQR2_deltaxsiant[0]=0;
c_rc_LQR2_xsi[0]=0;
c_rc_LQR2_xsiant[0]=0;
c_rc_LQR2_xsi[1]=0;
c_rc_LQR2_xsiant[1]=0;
c_rc_LQR2_deltaxsi[1]=0;
c_rc_LQR2_deltaxsiant[1]=0;
}else{
c_rc_LQR2_deltaxsi[0]=position.z-position_reference.z;
c_rc_LQR2_xsi[0]=c_rc_LQR2_xsiant[0]+(((float32_t)sample_time)*(c_rc_LQR2_deltaxsi[0]+c_rc_LQR2_deltaxsiant[0])*0.5);
c_rc_LQR2_deltaxsi[1]=attitude.yaw-attitude_reference.yaw;
c_rc_LQR2_xsi[1]=c_rc_LQR2_xsiant[1]+(((float32_t)sample_time)*(c_rc_LQR2_deltaxsi[1]+c_rc_LQR2_deltaxsiant[1])*0.5);
}
// anti_windup
c_rc_LQR2_xsi[0]=c_rc_LQR2_saturation(c_rc_LQR2_xsi[0],-0.2,0.2);
c_rc_LQR2_xsi[1]=c_rc_LQR2_saturation(c_rc_LQR2_xsi[1],-0.2,0.2);
//update
c_rc_LQR2_deltaxsiant[0]=c_rc_LQR2_deltaxsi[0]=0;
c_rc_LQR2_deltaxsiant[1]=c_rc_LQR2_deltaxsi[1]=0;
c_rc_LQR2_xsiant[1]=c_rc_LQR2_xsi[0];
c_rc_LQR2_xsiant[1]=c_rc_LQR2_xsi[1];
//State Vector
c_rc_LQR2_state_vector_I_f32[STATE_X]=position.x;
c_rc_LQR2_state_vector_I_f32[STATE_Y]=position.y;
c_rc_LQR2_state_vector_I_f32[STATE_Z]=position.z;
c_rc_LQR2_state_vector_I_f32[STATE_ROLL]=attitude.roll;
c_rc_LQR2_state_vector_I_f32[STATE_PITCH]=attitude.pitch;
c_rc_LQR2_state_vector_I_f32[STATE_YAW]=attitude.yaw;
c_rc_LQR2_state_vector_I_f32[STATE_ALPHA_R]=servo.alphar;
c_rc_LQR2_state_vector_I_f32[STATE_ALPHA_L]=servo.alphal;
c_rc_LQR2_state_vector_I_f32[STATE_DX]=position.dotX;
c_rc_LQR2_state_vector_I_f32[STATE_DY]=position.dotY;
c_rc_LQR2_state_vector_I_f32[STATE_DZ]=position.dotZ;
c_rc_LQR2_state_vector_I_f32[STATE_DROLL]=attitude.dotRoll;
c_rc_LQR2_state_vector_I_f32[STATE_DPITCH]=attitude.dotPitch;
c_rc_LQR2_state_vector_I_f32[STATE_DYAW]=attitude.dotYaw;
c_rc_LQR2_state_vector_I_f32[STATE_DALPHA_R]=servo.dotAlphar;
c_rc_LQR2_state_vector_I_f32[STATE_DALPHA_L]=servo.dotAlphal;
c_rc_LQR2_state_vector_I_f32[STATE_INT_Z]=c_rc_LQR2_xsi[0];
c_rc_LQR2_state_vector_I_f32[STATE_INT_YAW]=c_rc_LQR2_xsi[1];
//Updates the height equilibrium point according to the reference
c_rc_LQR2_state_vector_reference_I_f32[STATE_X]=position_reference.x;
c_rc_LQR2_state_vector_reference_I_f32[STATE_Y]=position_reference.y;
c_rc_LQR2_state_vector_reference_I_f32[STATE_Z]=position_reference.z;
c_rc_LQR2_state_vector_reference_I_f32[STATE_ROLL]=attitude_reference.roll;
c_rc_LQR2_state_vector_reference_I_f32[STATE_PITCH]=attitude_reference.pitch;
c_rc_LQR2_state_vector_reference_I_f32[STATE_YAW]=attitude_reference.yaw;
c_rc_LQR2_state_vector_reference_I_f32[STATE_ALPHA_R]=servo_reference.alphar;
c_rc_LQR2_state_vector_reference_I_f32[STATE_ALPHA_L]=servo_reference.alphal;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DX]=position_reference.dotX;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DY]=position_reference.dotY;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DZ]=position_reference.dotZ;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DROLL]=attitude_reference.dotRoll;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DPITCH]=attitude_reference.dotPitch;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DYAW]=attitude_reference.dotYaw;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DALPHA_R]=servo_reference.dotAlphar;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DALPHA_L]=servo_reference.dotAlphal;
c_rc_LQR2_state_vector_reference_I_f32[STATE_INT_Z]=0;
c_rc_LQR2_state_vector_reference_I_f32[STATE_INT_YAW]=0;
//Initializes the matrices
arm_mat_init_f32(&c_rc_LQR2_state_vector, 18, 1, (float32_t *)c_rc_LQR2_state_vector_I_f32);
arm_mat_init_f32(&c_rc_LQR2_reference_state_vector, 18, 1, (float32_t *)c_rc_LQR2_state_vector_reference_I_f32);
arm_mat_init_f32(&c_rc_LQR2_error_state_vector, 18, 1, (float32_t *)c_rc_LQR2_error_state_vector_I_f32);
//e(t)=x(k)- xr(k)
arm_mat_sub_f32(&c_rc_LQR2_state_vector, &c_rc_LQR2_reference_state_vector, &c_rc_LQR2_error_state_vector);
return c_rc_LQR2_error_state_vector;
}
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];
}
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);
}
