/* Perform the prediction step
Predict state
x_p = f(x);
A = Jacobian of f(x)
Predict P
P = A * P * A' + Q;
Predict measure
z_p = h(x_p)
H = Jacobian of h(x)
*/
void discrete_ekf_predict(struct discrete_ekf *filter)
{
float dX[EKF_N];
MAKE_MATRIX_PTR(_tmp1, filter->tmp1, EKF_N);
MAKE_MATRIX_PTR(_tmp2, filter->tmp2, EKF_N);
MAKE_MATRIX_PTR(_tmp3, filter->tmp3, EKF_N);
MAKE_MATRIX_PTR(_H, filter->H, EKF_N);
MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
MAKE_MATRIX_PTR(_Q, filter->Q, EKF_N);
// Fetch dX and A given X and dt and input u
linear_filter(filter->X, filter->dt, dX, _tmp1); // (note: _tmp1 = A)
// Get state prediction Xp = X + dX
float_vect_sum(filter->Xp, filter->X, dX, EKF_N);
// Get measurement prediction (Zp) and Jacobian (H)
linear_measure(filter->Xp, filter->Zp, _H);
// P = A * P * A' + Q
float_mat_mul(_tmp2, _tmp1, _P, EKF_N, EKF_N, EKF_N); // tmp2(=A*P) = A(=_tmp1)*_P
float_mat_transpose_square(_tmp1, EKF_N); // tmp1 = A'
float_mat_mul(_tmp3, _tmp2, _tmp1, EKF_N, EKF_N, EKF_N); // tmp3 = tmp2*tmp1 = A*P * A'
float_mat_sum(_P, _tmp3, _Q, EKF_N, EKF_N); // P = tmp3(=A*P*A') + Q
}
void loop_imu(void)
{
//IMU sensor data using Bluetooth printing
//Get ITG3205 data
READ_ITG3205_XYZT();
//Get ADXL345 data
ADXL345_Read_XYZt();
//Get HMC5883L data
HMC5883L_Read_XYZt();
evaluate_rotation_matrix_imu();
linear_filter();
delay_ms(50);
// sprintf((char *)AddSDstr,"\r\nG_X=%5d,G_Y=%5d,G_Z=%5d, Temp=%d \r\n\
A_X=%.4f,A_Y=%.4f,MAG=%.4f\r\n",ITG3205_X,ITG3205_Y,ITG3205_Z,ITG3205_T\
,ADXL345_X_AXIS,ADXL345_Y_AXIS,HMC5883L_ARC);
// Write_SD("gps_data.txt",AddSDstr,102400000);
//get GPS data
// // The value of "Print_Port" is 3 in the retarget.h (serial 3 printing)
//// //Initialize serial port 3 testing
//Modules_Power_Control(Zigbee_Power,1);
// USART_INIT(9600,3,ENABLE);
// // The value of "Print_Port" is 2 in the retarget.h (Bluetooth printing)
// //Initialize serial port 2 testing(blue)
// Modules_Power_Control(IMU_Blue_Power,1);
// USART_INIT(9600,2,ENABLE);
//GPS power on
//GPS_Parse_Decode();
}
void loop(void)
{
// FILINFO fno;
// uint32_t total,free;
// static FRESULT res;
// uint8_t AddSDstr[256];
// FIL log_file;
char file_name[20],read_buffer[10];
static uint8_t write_num=2;
static uint32_t file_num,i;
uint8_t AddSDstr[256];
//IMU sensor data using Bluetooth printing
//Get ITG3205 data
READ_ITG3205_XYZT();
//Get ADXL345 data
ADXL345_Read_XYZt();
//Get HMC5883L data
HMC5883L_Read_XYZt();
evaluate_rotation_matrix_imu();
linear_filter();
calculate_p_r_y();
GPS_Parse_Decode();
// //creat_file
if(write_num==2){
sprintf(file_name,"gps_recv_%d.txt",file_num++);
//´´½¨Îļþ creat file
if(SD_Creat_File(file_name))
printf("create file success\r\n");
else{
printf("create file fail\r\n");
while(1);
}
}
// //MAX file size is 100K ,file size> max_size create a file again return write_status ==2
//sprintf((char *)AddSDstr,"%d,%d,%d;%f,%f,%f\r\n",ITG3205_X,ITG3205_Y,ITG3205_Z,conver_x,conver_y,conver_z);
sprintf((char *)AddSDstr,"%d-%d-%d, %d:%d:%d; Ax = %f,Ay = %f,Az = %f, "
,info.utc.year,info.utc.mon,info.utc.day,info.utc.hour, info.utc.min,info.utc.sec,conver_x,conver_y,conver_z);
write_num=Write_SD(file_name,AddSDstr,102400);
sprintf((char *)AddSDstr,"Gx = %d,Gy = %d,Gz = %d, Latitude = %f, Longitude = %f\r\n ",ITG3205_X,ITG3205_Y,ITG3205_Z,info.lat,info.lon);
write_num=Write_SD(file_name,AddSDstr,102400);
Read_SD(file_name, (char*)read_buffer, 10*(i++),10);
USART_Write_LEN((u8 *)read_buffer,10,3);
delay_ms(100);
}
