/* 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); }