void run_ekf(struct tilt_data* td) {
/* model noise covariance matrix */
double Q[4]={0.001, 0.0,
0.0, 0.003};
/* measurement noise covariance matrix */
double R[1]={1.7};
/* initial x */
double X0[2] = {0.0, 0.0};
/* initial state covariance matrix */
double P0[4] = {1.0, 0.0,
0.0, 1.0};
/* measure */
double y[1];
/* command */
double u[1];
struct ekf_filter* filter;
filter = ekf_filter_new(2, 1, Q, R, linear_filter, linear_measure);
tilt_init(td, 150, X0);
ekf_filter_reset(filter, X0, P0);
/* filter run */
for (iter=0; iter<td->nb_samples; iter++) {
u[0] = td->gyro[iter];
y[0] = td->m_angle[iter];
ekf_filter_predict(filter, u);
ekf_filter_update(filter, y);
ekf_filter_get_state(filter, X0, P0);
tilt_data_save_state(td, iter, X0, P0);
}
}
/*
*perform kalman filter at time K, estimate best navigation parameter error at time K
*estimate navigation parameters at current time when acc bias estimation is stable
*/
void vIEKFProcessTask(void* pvParameters)
{
u8 i,j;
u8 acc_bias_stable = 0; //indicate whether acc bias is stably estimated
char logData[100]={0};
ekf_filter filter;
float dt;
float measure[5]={0};
float insBufferK[INS_FRAME_LEN];
float insBufferCur[INS_FRAME_LEN];
float *p_insBuffer;
float direction;
u8 temp;
GPSDataType gdt={0.0,0.0,0.0,0.0,0.0};
portBASE_TYPE xstatus;
PosConDataType pcdt; //position message send to flight control task
/*initial filter*/
filter=ekf_filter_new(9,5,(float *)iQ,(float *)iR,INS_GetA,INS_GetH,INS_aFunc,INS_hFunc);
memcpy(filter->x,x,filter->state_dim*sizeof(float));
memcpy(filter->P,iP,filter->state_dim*filter->state_dim*sizeof(float));
/*capture an INS frame*/
xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY);
/*last number in buffer represent time interval, not time */
p_insBuffer[INDEX_DT]=0.0;
Blinks(LED1,4);
for(;;)
{
/*capture an INS frame*/
xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY);
PutToBuffer(p_insBuffer);
p_insBuffer[INDEX_DT]=0.0;
ReadBufferBack(insBufferK);
dt=insBufferK[INDEX_DT];
/*do INS integration at time K*/
INS_Update(navParamK, insBufferK);
/*do INS integration at current time*/
if(acc_bias_stable)
{
ReadBufferFront(insBufferCur);
INS_Update(navParamCur,insBufferCur);
}
/*predict navigation error at time K*/
EKF_predict(filter
, (void *)(insBufferK+3)
, NULL
, (void *)(&dt)
, (void *)(filter->A)
, NULL);
xstatus=xQueueReceive(xUartGPSQueue,&gdt,0); //get measurement data
if(xstatus == pdPASS)
{
float meas_Err[5]={0.0};
direction = gdt.COG*0.0174533;
temp=(u8)(gdt.Lati*0.01);
measure[0]=(0.01745329*(temp+(gdt.Lati-temp*100.0)*0.0166667)-initPos[0])*6371004;
temp=(u8)(gdt.Long*0.01);
measure[1]=(0.01745329*(temp+(gdt.Long-temp*100.0)*0.0166667)-initPos[1])*4887077;
measure[2]=gdt.Alti-initPos[2];
measure[3]=gdt.SPD*0.51444*arm_cos_f32(direction);
measure[4]=gdt.SPD*0.51444*arm_sin_f32(direction);
for(i=0;i<5;i++)
{
meas_Err[i] = navParamK[i] - measure[i];
}
/*data record*/
/*uncomment this to record data for matlab simulation*/
// sprintf(logData,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",
// insBufferK[0],insBufferK[1],insBufferK[2],
// insBufferK[3],insBufferK[4],insBufferK[5],
// insBufferK[6],insBufferK[7],
// measure[0],measure[1],measure[2],measure[3],measure[4],
// navParamK[3],navParamK[4]);
// xQueueSend(xDiskLogQueue,logData,0);
sprintf(logData, "%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",
measure[0],measure[1],measure[3],measure[4],
navParamK[0],navParamK[1],navParamK[3],navParamK[4],
navParamCur[0],navParamCur[1],navParamCur[3],navParamCur[4]);
xQueueSend(xDiskLogQueue,logData,0);
/*update*/
EKF_update(filter
, (void *)meas_Err
, NULL
, NULL
, (void *)(filter->x)
, NULL);
// printf("%.2f %.2f %.4f %.4f %.4f\r\n",meas_Err[0],meas_Err[1],filter->x[6],filter->x[7],filter->x[8]);
/*
*correct navParamCur
*/
if(acc_bias_stable)
{
for(i=0;i<6;i++)
navParamCur[i] -= filter->x[i];
navParamCur[6] = filter->x[6];
navParamCur[7] = filter->x[7];
navParamCur[8] = filter->x[8];
pcdt.posX = navParamCur[0];
pcdt.posY = navParamCur[1];
pcdt.posZ = navParamCur[2];
pcdt.veloX = navParamCur[3];
pcdt.veloY = navParamCur[4];
pcdt.veloZ = navParamCur[5];
/*put to queue*/
xQueueSend(INSToFlightConQueue,&pcdt,0);
}
/*correct navParameters at time K
*reset error state x*/
for(i=0;i<6;i++)
{
navParamK[i] -= filter->x[i];
filter->x[i] = 0.0;
}
navParamK[6] = filter->x[6];
navParamK[7] = filter->x[7];
navParamK[8] = filter->x[8];
/*when acc bias stable,
*calculate current navigation parameters*/
if(!acc_bias_stable && filter->P[60]<0.007)
{
acc_bias_stable = 1;
//set init value
for(i=0;i<filter->state_dim;i++)
{
navParamCur[i] = navParamK[i];
}
//extrapolate current navigation parameters from time K
for(i=0;i<GPS_DELAY_CNT;i++)
{
if(i+buffer_header >= GPS_DELAY_CNT)
{
for(j=0;j<INS_FRAME_LEN;j++)
insBufferCur[j] = IMU_delay_buffer[(i+buffer_header-GPS_DELAY_CNT)*INS_FRAME_LEN+j];
}
else
{
for(j=0;j<INS_FRAME_LEN;j++)
insBufferCur[j] = IMU_delay_buffer[(i+buffer_header)*INS_FRAME_LEN+j];
}
insBufferCur[0] -= navParamK[6];
insBufferCur[1] -= navParamK[7];
insBufferCur[2] -= navParamK[8];
INS_Update(navParamCur,insBufferCur);
}
Blinks(LED1,1);
}
// printf("%.1f %.1f %.1f %.1f %.1f\r\n",navParamK[0],navParamK[1],navParamK[2],navParamK[3],navParamK[4]);
}
else
{
/*data record*/
sprintf(logData,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",
insBufferK[0],insBufferK[1],insBufferK[2],
insBufferK[3],insBufferK[4],insBufferK[5],
insBufferK[6],insBufferK[7],
0.0,0.0,0.0,0.0,0.0,
navParamK[3],navParamK[4]);
xQueueSend(xDiskLogQueue,logData,0);
}
}
}
void run_ekf (void) {
/* initial state covariance matrix */
double P[7*7] = {P0Q, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, P0Q, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, P0Q, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, P0Q, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, P0B, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, P0B, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, P0B };
/* model noise covariance matrix */
double Q[7*7] = {Q0G, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, Q0G, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, Q0G, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, Q0G, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, Q0B, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, Q0B, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Q0B };
/* measurement noise covariance matrix */
double R[1]={1.7};
/* state [q0, q1, q2, q3, bp, bq, br] */
double X[7];
/* measure */
double Y[1];
/* command */
double U[3] = {0.0, 0.0, 0.0};
struct ekf_filter* ekf;
ekf = ekf_filter_new(7, 1, Q, R, linear_filter, linear_measure);
ahrs_quat_init(ad, 150, X);
ekf_filter_reset(ekf, X, P);
/* filter run */
int iter;
for (iter=0; iter < ad->nb_samples; iter++) {
U[0] = ad->gyro_p[iter];// - X[4];
U[1] = ad->gyro_q[iter];// - X[5];
U[2] = ad->gyro_r[iter];// - X[6];
ekf_filter_predict(ekf, U);
norm_quat(X);
ahrs_state = iter%3;
switch (ahrs_state) {
case UPDATE_PHI:
Y[0] = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
break;
case UPDATE_THETA:
Y[0] = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
break;
case UPDATE_PSI: {
double eulers[3];
eulers_of_quat(eulers, X);
Y[0] = psi_of_mag(eulers[0], eulers[1], ad->mag_x[iter], ad->mag_y[iter], ad->mag_z[iter]);
break;
}
}
ekf_filter_update(ekf, Y);
norm_quat(X);
// printf("P66 %f\n", P[6*7 + 6]);
ekf_filter_get_state(ekf, X, P);
ahrs_data_save_state_quat(ad, iter, X, P);
ahrs_data_save_measure(ad, iter);
}
}
