纯C语言实现基于EKF算法(16维)的导航解算程序及数据

#include <stdio.h> #include <stdlib.h> #include "CMatrix.h" #include <windows.h> #include <math.h> #define DATA_LENGTH 11600 float large_quad_uncertainty = 0.1; float large_pos_uncertainty_m = 100.0f; float large_vel_uncertainty_mps = 10.0f; float ekf_sigmas_gyro_bias_rps = 0.01; float ekf_sigmas_accel_bias_mps2 = 0.1; float ekf_sigmas_quad_process_noise = 0.001; float ekf_sigmas_pos_process_noise_m = 0.1; float ekf_sigmas_vel_process_noise_mps = 0.1; float ekf_sigmas_gyroBias_process_noise_rps = 1e-5; float ekf_sigmas_accelBias_process_noise_mps2 = 1e-5; float ekf_sigmas_mag3D_unitVector_meas = 1.0f; float ekf_sigmas_pos_meas_m = 2.0f; int main(void) { ////时间相关 //double run_time; //LARGE_INTEGER time_start; //开始时间 //LARGE_INTEGER time_over; //结束时间 //double dqFreq; //计时器频率 //LARGE_INTEGER f; //计时器频率 //QueryPerformanceFrequency(&f); //dqFreq = (double)f.QuadPart; //传感器数据 float accx[DATA_LENGTH], accy[DATA_LENGTH], accz[DATA_LENGTH]; float gyrox[DATA_LENGTH], gyroy[DATA_LENGTH], gyroz[DATA_LENGTH]; float magx[DATA_LENGTH], magy[DATA_LENGTH], magz[DATA_LENGTH]; float GPS_east_m[DATA_LENGTH], GPS_north_m[DATA_LENGTH], alt[DATA_LENGTH]; float roll_deg_ref[DATA_LENGTH], pitch_deg_ref[DATA_LENGTH], yaw_deg_ref[DATA_LENGTH]; float roll_deg[DATA_LENGTH], pitch_deg[DATA_LENGTH], yaw_deg[DATA_LENGTH]; //读取CSV原始数据 char filename_read[] = ".\\9DOF_DATA.csv"; char filename_write[] = ".\\DATA.csv"; char line[1024]; char *record; MatDataType data_temp; FILE *fp_read = NULL; FILE *fp_write = NULL; fp_read = fopen(filename_read,"r"); fp_write = fopen(filename_write, "a"); //前两行数据无效 fgets(line, 1024, fp_read); fgets(line, 1024, fp_read); fgets(line, 1024, fp_read); for (int i = 0; i < DATA_LENGTH; i++) { fgets(line, 1024, fp_read); for (int j = 0; j < 12; j++) { if (j == 0) { record = strtok(line, ","); data_temp = atof(record); accx[i] = data_temp * 0.001; //printf("accx:%lf\t", data_temp); } if (j == 1) { record = strtok(NULL, ","); data_temp = atof(record); accy[i] = data_temp * 0.001; //printf("accy:%lf\t", data_temp); } if (j == 2) { record = strtok(NULL, ","); data_temp = atof(record); accz[i] = - data_temp * 0.001; //printf("accz:%lf\t", data_temp); } if (j == 3) { record = strtok(NULL, ","); data_temp = atof(record); gyrox[i] = data_temp * 0.001; //printf("gyrox:%lf\t", data_temp); } if (j == 4) { record = strtok(NULL, ","); data_temp = atof(record); gyroy[i] = data_temp * 0.001; //printf("gyroy:%lf\t", data_temp); } if (j == 5) { record = strtok(NULL, ","); data_temp = atof(record); gyroz[i] = data_temp * 0.001; //printf("gyroz:%lf\t", data_temp); } if (j == 6) { record = strtok(NULL, ","); data_temp = atof(record); magx[i] = data_temp * 0.1; //printf("magx:%lf\t", data_temp); } if (j == 7) { record = strtok(NULL, ","); data_temp = atof(record); magy[i] = data_temp * 0.1; //printf("magy:%lf\t", data_temp); } if (j == 8) { record = strtok(NULL, ","); data_temp = atof(record); magz[i] = data_temp * 0.1; //printf("magz:%lf\t", data_temp); } if (j == 9) { record = strtok(NULL, ","); data_temp = atof(record); roll_deg[i] = data_temp; //printf("roll_deg:%lf\t", data_temp); } if (j == 10) { record = strtok(NULL, ","); data_temp = atof(record); pitch_deg[i] = data_temp; //printf("pitch_deg:%lf\t", data_temp); } if (j == 11) { record = strtok(NULL, ","); data_temp = atof(record); yaw_deg[i] = data_temp; //printf("i:%d yaw_deg:%lf\n",i, yaw_deg[i]); } } // GPS_east_m[i] = 0.0f; GPS_north_m[i] = 0.0f; alt[i] = 3.0f; } fclose(fp_read); //常量 float gravity_mps2 = 9.81; //重力加速度 float mag_declination_deg = 0; //磁偏角 //状态初始值 Mat ekf_xhat; MatCreate(&ekf_xhat, 16, 1); MatDataType ekf_xhat_init[16] = { 1.0f,0.0f,0.0f,0.0f, 0.0f,0.0f,3.0f, 0.0f,0.0f,0.0f, 0.0f,0.0f,0.0f, 0.0f,0.0f,0.0f }; MatInit(&ekf_xhat, ekf_xhat_init); //P矩阵初始化 Mat ekf_P; MatCreate(&ekf_P, 16, 16); MatDataType ekf_P_init[16] = { large_quad_uncertainty,large_quad_uncertainty,large_quad_uncertainty,large_quad_uncertainty,\ large_pos_uncertainty_m , large_pos_uncertainty_m , large_pos_uncertainty_m ,\ large_vel_uncertainty_mps , large_vel_uncertainty_mps , large_vel_uncertainty_mps ,\ ekf_sigmas_gyro_bias_rps , ekf_sigmas_gyro_bias_rps , ekf_sigmas_gyro_bias_rps ,\ ekf_sigmas_accel_bias_mps2 , ekf_sigmas_accel_bias_mps2 , ekf_sigmas_accel_bias_mps2 }; MatDiag(&ekf_P, ekf_P_init, 16); //MatPrint(&ekf_P); //Q矩阵初始化 Mat ekf_Q; MatCreate(&ekf_Q, 16, 16); MatDataType ekf_Q_init[16] = { ekf_sigmas_quad_process_noise,ekf_sigmas_quad_process_noise ,ekf_sigmas_quad_process_noise ,ekf_sigmas_quad_process_noise, \ ekf_sigmas_pos_process_noise_m , ekf_sigmas_pos_process_noise_m , ekf_sigmas_pos_process_noise_m, \ ekf_sigmas_vel_process_noise_mps , ekf_sigmas_vel_process_noise_mps , ekf_sigmas_vel_process_noise_mps, \ ekf_sigmas_gyroBias_process_noise_rps , ekf_sigmas_gyroBias_process_noise_rps , ekf_sigmas_gyroBias_process_noise_rps ,\ ekf_sigmas_accelBias_process_noise_mps2 , ekf_sigmas_accelBias_process_noise_mps2 , ekf_sigmas_accelBias_process_noise_mps2 }; MatDiag(&ekf_Q, ekf_Q_init, 16); //MatPrint(&ekf_Q); //R矩阵初始化 Mat ekf_R; MatCreate(&ekf_R, 6, 6); MatDataType ekf_R_init[6] = { ekf_sigmas_mag3D_unitVector_meas,ekf_sigmas_mag3D_unitVector_meas,ekf_sigmas_mag3D_unitVector_meas,\ ekf_sigmas_pos_meas_m, ekf_sigmas_pos_meas_m, ekf_sigmas_pos_meas_m }; MatDiag(&ekf_R, ekf_R_init,6); MatDataType q0,q1,q2,q3; MatDataType Pn,Pe,Alt; MatDataType Vn,Ve,Vd; MatDataType bwx,bwy,bwz; MatDataType bax, bay, baz; //MatPrint(&ekf_R); for (int i = 0; i < DATA_LENGTH; i++) { float dt_s = 0.01; //Xhat int nStates = 16; q0 = MatAt(&ekf_xhat, 0, 0), q1 = MatAt(&ekf_xhat, 1, 0), q2 = MatAt(&ekf_xhat, 2, 0), q3 = MatAt(&ekf_xhat, 3, 0); Pn = MatAt(&ekf_xhat, 4, 0), Pe = MatAt(&ekf_xhat, 5, 0), Alt = MatAt(&ekf_xhat, 6, 0); Vn = MatAt(&ekf_xhat, 7, 0), Ve = MatAt(&ekf_xhat, 8, 0), Vd = MatAt(&ekf_xhat, 9, 0); bwx = MatAt(&ekf_xhat, 10, 0), bwy = MatAt(&ekf_xhat, 11, 0), bwz = MatAt(&ekf_xhat, 12, 0); bax = MatAt(&ekf_xhat, 13, 0), bay = MatAt(&ekf_xhat, 14, 0), baz = MatAt(&ekf_xhat, 15, 0); //角速度测量值 float wx = gyrox[i], wy = gyroy[i], wz = gyroz[i]; Mat Wxyz; MatCreate(&Wxyz, 3, 1); MatDataType Wxyz_data[3] = { wx,wy,wz }; MatInit(&Wxyz, Wxyz_data); //加表测量值 float fx = accx[i], fy = accy[i], fz = accz[i]; Mat Fxyz; MatCreate(&Fxyz, 3, 1); MatDataType Fxyz_data[3] = { fx,fy,fz }; MatInit(&Fxyz, Fxyz_data); //陀螺仪三轴偏置 Mat Bw; MatCreate(&Bw, 3, 1); MatDataType Bw_data[3] = { bwx,bwy,bwz }; MatInit(&Bw, Bw_data); //加表三轴偏置 Mat Bf; MatCreate(&Bf, 3, 1); MatDataType Bf_data[3] = { bax,bay,baz }; MatInit(&Bf, Bf_data); //重力向量 Mat Gravity; MatCreate(&Gravity, 3, 1); MatDataType Gravity_data[3] = { 0.0f,0.0f,gravity_mps2 }; MatInit(&Gravity, Gravity_data); //计算xdot Mat C_bodyrate2qdot; MatCreate(&C_bodyrate2qdot, 4, 3); MatDataType C_bodyrate2qdot_data[12] = { -0.5*q1, -0.5*q2, -0.5*q3,\ 0.5*q0, -0.5*q3, 0.5*q2,\ 0.5*q3, 0.5*q0, -0.5*q1,\ -0.5*q2, 0.5*q1, 0.5*q0 }; MatInit(&C_bodyrate2qdot, C_bodyrate2qdot_data); // Mat C_ned2b; MatCreate(&C_ned2b,3,3); MatDataType C_ned2b_data[9] = { 1-2*(q2*q2 + q3*q3), 2*(q1*q2 + q3*q0), 2*(q1*q3 - q2*q0),\ 2*(q1*q2 - q3*q0), 1-2*(q1*q1 + q3*q3), 2*(q2*q3 + q1*q0),\