diff --git a/Wheel-INS/Const.h b/Wheel-INS/Const.h deleted file mode 100644 index 3e3f2a1..0000000 --- a/Wheel-INS/Const.h +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#define nStates 21 - -#define S_2_(a) ((a)*(a)) -#define absf(a) ((a)>0 ? (a):-(a)) -#define NormG (9.782940329221166) -#define PI (3.14159265358979323846) -#define D2R (PI/180.0) -#define R2D (180.0/PI) - -#define Latitude (30.0*D2R) -#define DaRate (200) -#define Half_dt (0.5f / DaRate) -#define BufLen (DaRate) -#define DaColNum (7) - -#define A_WGS84 6378137.0 -#define F_WGS84 1.0/298.257223563 // Flattening; WGS-84 -#define Earth_e2 0.00669437999013 -#define Omega_WGS 7.2921151467e-5 //[rad/s], the earth rotation rate - - -const int GmeanLen = (int)((DaRate / 2) + 1);//101 -const int Half_GmeanLen = (int)(GmeanLen / 2.0f); -#define HisLen (DaRate - Half_GmeanLen - 1) - -//===============================Initialization================================ -const double AlignLen = 10.0; -const double set_ini_pos[3] = { 0.0, 0.0, 0.0 }; -const double set_ini_vel[3] = { 0.0, 0.0, 0.0 }; -const double Init_Pos_Var[3] = { S_2_(0.05), S_2_(0.05), S_2_(0.05) }; -const double Init_Vel_Var[3] = { S_2_(0.05), S_2_(0.05), S_2_(0.05) }; -const double Init_Att_Var[3] = { S_2_(1.0*D2R), S_2_(1.0*D2R), S_2_(0.1*D2R) }; - -#define MATRIX_INVERSION_ERROR 1 - -typedef double Vec3[3]; -typedef double Vec7[7]; -typedef double QuatVec[4]; -typedef double Mat3[9]; \ No newline at end of file diff --git a/Wheel-INS/DataType.h b/Wheel-INS/DataType.h deleted file mode 100644 index 6eada7d..0000000 --- a/Wheel-INS/DataType.h +++ /dev/null @@ -1,72 +0,0 @@ -#pragma once - -#include "Const.h" - -struct S_NavState //Navigation struct -{ - - double Q[nStates][nStates];//noise matrix - double P[nStates][nStates];//Covariance matrix - double X[nStates];//State corrections - - Vec3 BLH; - Vec3 Pos; - Vec3 Vel; - Vec3 Att; - QuatVec Q_bn; - Mat3 C_bn; - Mat3 C_bv; - Mat3 C_vn; - Vec3 Bg; - Vec3 Sg; - Vec3 Ba; - Vec3 Sa; - - int InitFlag; - int InitSum; - double MemoInitDa[6]; - - double T_Cur; - double T_Zupt; - double T_Odo; - - double dt; - double PreMeas[6]; - double Pre_PreMeas[3]; - - int FilterUpdate; - - double Gx_comped; - - double vehiclepitch; - - double SysVodo; -}; - -struct IMU_ConstPara //IMU paras -{ - std::string filepath; - std::string filename; - - int MountPos; - - double SetIniHeading; - double SetMisalign[3]; - - double BgVar; - double BaVar; - double SgVar; - double SaVar; - double ARW_2; - double VRW_2; - double Tao_t; - - bool Odo_IfUse; - double Odo_dt; - double Odo_Var[3]; - - bool IfcompV; - - double WheelLeverArm[3]; - double Wheel_Radius; -}; \ No newline at end of file diff --git a/Wheel-INS/IMU_CFG.cpp b/Wheel-INS/IMU_CFG.cpp deleted file mode 100644 index ec246fe..0000000 --- a/Wheel-INS/IMU_CFG.cpp +++ /dev/null @@ -1,74 +0,0 @@ -#include "NavFunc.h" - - -void getIMUparas(IMU_ConstPara * IMU, cv::FileStorage fs, cv::FileNode IMU_paras) -{ - cv::FileNodeIterator it1 = IMU_paras.begin(); - cv::FileNodeIterator it = ++IMU_paras.begin(); - cv::FileNodeIterator it2 = --IMU_paras.end(); - std::vector leverarm, odo_std; - - IMU->filepath = (std::string)(*it1)["Filepath"]; - IMU->filename = (std::string)(*it1)["Filename"]; - - IMU->BgVar = S_2_(((double)(*it)["std_bg"]) / 3600.0*D2R); - IMU->BaVar = S_2_(((double)(*it)["std_ba"])*1e-5); - IMU->SgVar = S_2_(((double)(*it)["std_sg"])*1e-5); - IMU->SaVar = S_2_(((double)(*it)["std_sa"])*1e-5); - IMU->ARW_2 = S_2_(((double)(*it)["ARW"]) / 60.0 * D2R); - IMU->VRW_2 = S_2_(((double)(*it)["VRW"]) / 60.0); - IMU->Tao_t = ((double)(*it)["Tao_t"]);//Correlation time - - IMU->Odo_IfUse = (int)(*it)["ifODO"]; - - IMU->IfcompV = (int)fs["ifCompVel"]; - - if (IMU->MountPos != 0) - { - (*it2)["WheelLA"] >> leverarm; - } - (*it2)["ODO_std"] >> odo_std; - - if (IMU->MountPos != 0) - { - IMU->WheelLeverArm[0] = leverarm[0]; - IMU->WheelLeverArm[1] = leverarm[1]; - IMU->WheelLeverArm[2] = leverarm[2]; - } - - IMU->Odo_dt = (double)fs["ODO_dt"]; - IMU->Odo_Var[0] = S_2_(odo_std[0]); - IMU->Odo_Var[1] = S_2_(odo_std[1]); - IMU->Odo_Var[2] = S_2_(odo_std[2]); - - IMU->SetIniHeading = ((double)(*it2)["init_heading"])*D2R; - - IMU->Wheel_Radius = fs["Wheel_Radius"]; -} - -void Configure_IMU_Para(IMU_ConstPara * IMU, cv::FileStorage fs) -{ - std::string sysflag = (std::string)fs["sysflag"]; - cv::FileNode IMU_paras; - - if (strcmp(sysflag.c_str(), "r") == 0) - { - IMU_paras = fs["IMU_right"]; - IMU->MountPos = 1; - } - else if (strcmp(sysflag.c_str(), "l") == 0) - { - IMU_paras = fs["IMU_left"]; - IMU->MountPos = -1; - } - else - { - printf("SYSFLAG ERROR��\n"); - system("pause"); - return; - } - getIMUparas(IMU, fs, IMU_paras); - - fs.release(); -} - diff --git a/Wheel-INS/NavFunc.cpp b/Wheel-INS/NavFunc.cpp deleted file mode 100644 index bd2f585..0000000 --- a/Wheel-INS/NavFunc.cpp +++ /dev/null @@ -1,737 +0,0 @@ -#include -#include -#include -#include -#include -#include "NavFunc.h" - - -int Read_oneEpoch(FILE *fp, double da[]) -{ - int rc; - if ((rc = fread(&da[0], sizeof(double), DaColNum, fp)) != 0) - { - return 1; - } - else - { - return 0; - } -} - - -void Initial_Nav(double stime, double cur_meas[], IMU_ConstPara *imu, S_NavState *nav) -{ - nav->T_Cur = cur_meas[0]; - if (nav->T_Cur < stime + AlignLen) - { - if (nav->InitSum == 0) - { - zero_array(6, nav->MemoInitDa); - } - for (int i = 0; i < 6; i++) - { - nav->MemoInitDa[i] += cur_meas[i + 1]; - } - nav->InitSum++; - } - else - { - nav->InitFlag = 1; - for (int i = 0; i < 6; i++) - { - nav->MemoInitDa[i] /= nav->InitSum; - } - - //init the EKF - zero_array(nStates, &nav->X[0]); - zero_array(nStates*nStates, &nav->P[0][0]); - zero_array(nStates*nStates, &nav->Q[0][0]); - - nav->Q[3][3] = imu->VRW_2; - nav->Q[4][4] = imu->VRW_2; - nav->Q[5][5] = imu->VRW_2; - nav->Q[6][6] = imu->ARW_2; - nav->Q[7][7] = imu->ARW_2; - nav->Q[8][8] = imu->ARW_2; - nav->Q[9][9] = 2.0 * imu->BgVar / imu->Tao_t; - nav->Q[10][10] = 2.0 * imu->BgVar / imu->Tao_t; - nav->Q[11][11] = 2.0 * imu->BgVar / imu->Tao_t; - nav->Q[12][12] = 2.0 * imu->BaVar / imu->Tao_t; - nav->Q[13][13] = 2.0 * imu->BaVar / imu->Tao_t; - nav->Q[14][14] = 2.0 * imu->BaVar / imu->Tao_t; - nav->Q[15][15] = 2.0 * imu->SgVar / imu->Tao_t; - nav->Q[16][16] = 2.0 * imu->SgVar / imu->Tao_t; - nav->Q[17][17] = 2.0 * imu->SgVar / imu->Tao_t; - nav->Q[18][18] = 2.0 * imu->SaVar / imu->Tao_t; - nav->Q[19][19] = 2.0 * imu->SaVar / imu->Tao_t; - nav->Q[20][20] = 2.0 * imu->SaVar / imu->Tao_t; - - nav->P[0][0] = Init_Pos_Var[0]; - nav->P[1][1] = Init_Pos_Var[1]; - nav->P[2][2] = Init_Pos_Var[2]; - nav->P[3][3] = Init_Vel_Var[0]; - nav->P[4][4] = Init_Vel_Var[1]; - nav->P[5][5] = Init_Vel_Var[2]; - nav->P[6][6] = Init_Att_Var[0]; - nav->P[7][7] = Init_Att_Var[1]; - nav->P[8][8] = Init_Att_Var[2]; - nav->P[9][9] = imu->BgVar; - nav->P[10][10] = imu->BgVar; - nav->P[11][11] = imu->BgVar; - nav->P[12][12] = imu->BaVar; - nav->P[13][13] = imu->BaVar; - nav->P[14][14] = imu->BaVar; - nav->P[15][15] = imu->SgVar; - nav->P[16][16] = imu->SgVar; - nav->P[17][17] = imu->SgVar; - nav->P[18][18] = imu->SaVar; - nav->P[19][19] = imu->SaVar; - nav->P[20][20] = imu->SaVar; - - - nav->Pos[0] = set_ini_pos[0]; - nav->Pos[1] = set_ini_pos[1]; - nav->Pos[2] = set_ini_pos[2]; - - nav->Vel[0] = set_ini_vel[0]; - nav->Vel[1] = set_ini_vel[1]; - nav->Vel[2] = set_ini_vel[2]; - - nav->Att[0] = atan2(-nav->MemoInitDa[4], -nav->MemoInitDa[5]); - nav->Att[1] = atan2(nav->MemoInitDa[3], sqrt(nav->MemoInitDa[4] * nav->MemoInitDa[4] + nav->MemoInitDa[5] * nav->MemoInitDa[5])); - nav->Att[2] = imu->SetIniHeading; - - euler2rotation(nav->C_bn, nav->Att); - euler2quat(nav->Q_bn, nav->Att); - - zero_array(9, nav->C_vn); - - zero_array(3, &nav->Bg[0]); - zero_array(3, &nav->Ba[0]); - zero_array(3, &nav->Sg[0]); - zero_array(3, &nav->Sa[0]); - - //set initial gyro bias - nav->Bg[0] = nav->MemoInitDa[0]; - nav->Bg[1] = nav->MemoInitDa[1]; - nav->Bg[2] = nav->MemoInitDa[2]; - - nav->T_Odo = cur_meas[0]; - - zero_array(3, &nav->Pre_PreMeas[0]); - zero_array(6, &nav->PreMeas[0]); - - nav->SysVodo = 0; - nav->FilterUpdate = 0; - nav->Gx_comped = 0.0; - nav->vehiclepitch = 0.0; - - } -} - - -void Bias_Compensate(double cur_meas[], IMU_ConstPara *imu, S_NavState *nav) -{ - cur_meas[1] = (1 - nav->Sg[0])*(cur_meas[1] - nav->Bg[0]); - cur_meas[2] = (1 - nav->Sg[1])*(cur_meas[2] - nav->Bg[1]); - cur_meas[3] = (1 - nav->Sg[2])*(cur_meas[3] - nav->Bg[2]); - - cur_meas[4] = (1 - nav->Sa[0])*(cur_meas[4] - nav->Ba[0]); - cur_meas[5] = (1 - nav->Sa[1])*(cur_meas[5] - nav->Ba[1]); - cur_meas[6] = (1 - nav->Sa[2])*(cur_meas[6] - nav->Ba[2]); -} - -//The INS Mechanization algorithm adopted in this system is a simplified version. Because we focused on MEMS IMU and local robot positioning applications, we ignored the earth rotation, the change of naviagtion frame and gravity, which are important for high-end IMU and large-scale applications. -void INS_Mech(double pre_meas[], double cur_meas[], S_NavState *nav) -{ - - double dt = nav->dt; - - Vec3 acc_dt, an_hat, zeta, gyros_dt; - QuatVec quat_tmp; - double cos_v, sin_v; - - - Vec3 Gzeta, Gzeta1, Gzeta2; - - gyros_dt[0] = 0.5 * (pre_meas[1] + cur_meas[1]) * dt; - gyros_dt[1] = 0.5 * (pre_meas[2] + cur_meas[2]) * dt; - gyros_dt[2] = 0.5 * (pre_meas[3] + cur_meas[3]) * dt; - - - CrossProduct(nav->PreMeas, gyros_dt, Gzeta); - - //second-order - Gzeta[0] = gyros_dt[0] + Gzeta[0] / 12.0; - Gzeta[1] = gyros_dt[1] + Gzeta[1] / 12.0; - Gzeta[2] = gyros_dt[2] + Gzeta[2] / 12.0; - - //In the following comments, delta_ means the error of a variable, while Delta_ means the increment of a variable in a certain time interval. - double v = vec_norm(3, Gzeta); - if (v >1e-12) - { - cos_v = cos(v / 2.0); - sin_v = (sin(v / 2.0) / v); - //R[delta_q]*Q_bn (Sola,P7) delta_q = Q_bk_bk-1 - quat_tmp[0] = cos_v*nav->Q_bn[0] + sin_v*(Gzeta[2] * nav->Q_bn[1] - Gzeta[1] * nav->Q_bn[2] + Gzeta[0] * nav->Q_bn[3]); - quat_tmp[1] = cos_v*nav->Q_bn[1] + sin_v*(-Gzeta[2] * nav->Q_bn[0] + Gzeta[0] * nav->Q_bn[2] + Gzeta[1] * nav->Q_bn[3]); - quat_tmp[2] = cos_v*nav->Q_bn[2] + sin_v*(Gzeta[1] * nav->Q_bn[0] - Gzeta[0] * nav->Q_bn[1] + Gzeta[2] * nav->Q_bn[3]); - quat_tmp[3] = cos_v*nav->Q_bn[3] + sin_v*(-Gzeta[0] * nav->Q_bn[0] - Gzeta[1] * nav->Q_bn[1] - Gzeta[2] * nav->Q_bn[2]); - - v = vec_norm(4, quat_tmp); - nav->Q_bn[0] = quat_tmp[0] / v; - nav->Q_bn[1] = quat_tmp[1] / v; - nav->Q_bn[2] = quat_tmp[2] / v; - nav->Q_bn[3] = quat_tmp[3] / v; - - - quat2rotation(nav->C_bn, nav->Q_bn); - rotation2euler(nav->Att, nav->C_bn); - } - - //Delta_v_fk_b - acc_dt[0] = 0.5 * (pre_meas[4] + cur_meas[4]) * dt; - acc_dt[1] = 0.5 * (pre_meas[5] + cur_meas[5]) * dt; - acc_dt[2] = 0.5 * (pre_meas[6] + cur_meas[6]) * dt; - //Delta_v_fk_b + (Delta_theta_k (cross product) Delta_v_fk_b)/2 - zeta[0] = acc_dt[0] + (-gyros_dt[2] * acc_dt[1] + gyros_dt[1] * acc_dt[2]) / 2.0; - zeta[1] = acc_dt[1] + (gyros_dt[2] * acc_dt[0] - gyros_dt[0] * acc_dt[2]) / 2.0; - zeta[2] = acc_dt[2] + (-gyros_dt[1] * acc_dt[0] + gyros_dt[0] * acc_dt[1]) / 2.0; - //Delta_v_fk_b(k-1)= Delta_v_fk_b + (Delta_theta_k (cross product) Delta_v_fk_b)/2 + (Delta_theta_k-1 (cross product) Delta_v_fk_b + Delta_v_fk_b (cross product) Delta_theta_k)/12 - zeta[0] += (-nav->PreMeas[2] * acc_dt[1] + nav->PreMeas[1] * acc_dt[2] - nav->PreMeas[5] * gyros_dt[1] + nav->PreMeas[4] * gyros_dt[2]) / 12.0; - zeta[1] += (nav->PreMeas[2] * acc_dt[0] - nav->PreMeas[0] * acc_dt[2] + nav->PreMeas[5] * gyros_dt[0] - nav->PreMeas[3] * gyros_dt[2]) / 12.0; - zeta[2] += (-nav->PreMeas[1] * acc_dt[0] + nav->PreMeas[0] * acc_dt[1] - nav->PreMeas[4] * gyros_dt[0] + nav->PreMeas[3] * gyros_dt[1]) / 12.0; - - Mat3 Half_gros_dt; - Half_gros_dt[0] = 1.0; Half_gros_dt[1] = 0.5*gyros_dt[2]; Half_gros_dt[2] = -0.5*gyros_dt[1]; - Half_gros_dt[3] = -0.5*gyros_dt[2]; Half_gros_dt[4] = 1.0; Half_gros_dt[5] = 0.5*gyros_dt[0]; - Half_gros_dt[6] = 0.5*gyros_dt[1]; Half_gros_dt[7] = -0.5*gyros_dt[0]; Half_gros_dt[8] = 1.0; - Mat3 Half_Cbn; - MultiplyMatrix(nav->C_bn, Half_gros_dt, 3, 3, 3, Half_Cbn); //Cbn*[I - (0.5*skewmatrix(Delta_theta_k))] - - //Delta_v_nk = C_b(k-1)n * Delta_v_b(k-1) + g_n * dt - an_hat[0] = Half_Cbn[0] * zeta[0] + Half_Cbn[1] * zeta[1] + Half_Cbn[2] * zeta[2]; - an_hat[1] = Half_Cbn[3] * zeta[0] + Half_Cbn[4] * zeta[1] + Half_Cbn[5] * zeta[2]; - an_hat[2] = Half_Cbn[6] * zeta[0] + Half_Cbn[7] * zeta[1] + Half_Cbn[8] * zeta[2] + NormG * dt; - - // P_nk = P_nk-1 + (v_nk-1 + Delta_v_nk/2)*dt - nav->Pos[0] = nav->Pos[0] + nav->Vel[0] * dt + 0.5*an_hat[0] * dt; - nav->Pos[1] = nav->Pos[1] + nav->Vel[1] * dt + 0.5*an_hat[1] * dt; - nav->Pos[2] = nav->Pos[2] + nav->Vel[2] * dt + 0.5*an_hat[2] * dt; - - nav->Vel[0] = nav->Vel[0] + an_hat[0]; - nav->Vel[1] = nav->Vel[1] + an_hat[1]; - nav->Vel[2] = nav->Vel[2] + an_hat[2]; - - - nav->Pre_PreMeas[0] = nav->PreMeas[0]; - nav->Pre_PreMeas[1] = nav->PreMeas[1]; - nav->Pre_PreMeas[2] = nav->PreMeas[2]; - - nav->PreMeas[0] = gyros_dt[0]; - nav->PreMeas[1] = gyros_dt[1]; - nav->PreMeas[2] = gyros_dt[2]; - - nav->PreMeas[3] = acc_dt[0]; - nav->PreMeas[4] = acc_dt[1]; - nav->PreMeas[5] = acc_dt[2]; -} - - -void EKF_Predict(double cur_meas[], IMU_ConstPara *imu, S_NavState *nav) -{ - double dt = nav->dt; - double Pp[nStates][nStates]; - int i = 0, j = 0; - for (i = 0; i < nStates; i++) - { - for (j = 0; j < nStates; j++) - { - Pp[i][j] = nav->P[i][j]; - } - } - Vec3 f_n; //f_n = (C_bn*f_b) - f_n[0] = nav->C_bn[0] * cur_meas[4] + nav->C_bn[1] * cur_meas[5] + nav->C_bn[2] * cur_meas[6]; - f_n[1] = nav->C_bn[3] * cur_meas[4] + nav->C_bn[4] * cur_meas[5] + nav->C_bn[5] * cur_meas[6]; - f_n[2] = nav->C_bn[6] * cur_meas[4] + nav->C_bn[7] * cur_meas[5] + nav->C_bn[8] * cur_meas[6]; - - double PHI[nStates][nStates] = { 0.0 }; - double PHI_T[nStates][nStates] = { 0.0 }; - double PHI_P[nStates][nStates] = { 0.0 }; - double Q_w[nStates][nStates] = { 0.0 }; - double tem_matrix[nStates][nStates] = { 0.0 }; - - //set the value of PHI=I+F*dt - if (nStates >= 9) - { - for (i = 0; i < 9; i++) - { - PHI[i][i] = 1.0; - } - // I - PHI[0][3] = dt; - PHI[1][4] = dt; - PHI[2][5] = dt; - - // dt*skewmatrix(C_bn*f_b) - PHI[3][7] = -f_n[2] * dt; PHI[3][8] = f_n[1] * dt; - PHI[4][6] = f_n[2] * dt; PHI[4][8] = -f_n[0] * dt; - PHI[5][6] = -f_n[1] * dt; PHI[5][7] = f_n[0] * dt; - - for (i = 9; i < 15; i++) - { - PHI[i][i] = exp(-dt / imu->Tao_t);//exp(F(t)dt) - } - // C_bn - PHI[3][12] = nav->C_bn[0] * dt; PHI[3][13] = nav->C_bn[1] * dt; PHI[3][14] = nav->C_bn[2] * dt; - PHI[4][12] = nav->C_bn[3] * dt; PHI[4][13] = nav->C_bn[4] * dt; PHI[4][14] = nav->C_bn[5] * dt; - PHI[5][12] = nav->C_bn[6] * dt; PHI[5][13] = nav->C_bn[7] * dt; PHI[5][14] = nav->C_bn[8] * dt; - - // -C_bn - PHI[6][9] = -nav->C_bn[0] * dt; PHI[6][10] = -nav->C_bn[1] * dt; PHI[6][11] = -nav->C_bn[2] * dt; - PHI[7][9] = -nav->C_bn[3] * dt; PHI[7][10] = -nav->C_bn[4] * dt; PHI[7][11] = -nav->C_bn[5] * dt; - PHI[8][9] = -nav->C_bn[6] * dt; PHI[8][10] = -nav->C_bn[7] * dt; PHI[8][11] = -nav->C_bn[8] * dt; - - for (i = 15; i < 21; i++) - { - PHI[i][i] = exp(-dt / imu->Tao_t);//exp(F(t)dt) - } - // C_bn*diag(fb) - PHI[3][18] = nav->C_bn[0] * cur_meas[4] * dt; PHI[3][19] = nav->C_bn[1] * cur_meas[5] * dt; PHI[3][20] = nav->C_bn[2] * cur_meas[6] * dt; - PHI[4][18] = nav->C_bn[3] * cur_meas[4] * dt; PHI[4][19] = nav->C_bn[4] * cur_meas[5] * dt; PHI[4][20] = nav->C_bn[5] * cur_meas[6] * dt; - PHI[5][18] = nav->C_bn[6] * cur_meas[4] * dt; PHI[5][19] = nav->C_bn[7] * cur_meas[5] * dt; PHI[5][20] = nav->C_bn[8] * cur_meas[6] * dt; - // -C_bn*diag(wibb) - PHI[6][15] = -nav->C_bn[0] * cur_meas[1] * dt; PHI[6][16] = -nav->C_bn[1] * cur_meas[2] * dt; PHI[6][17] = -nav->C_bn[2] * cur_meas[3] * dt; - PHI[7][15] = -nav->C_bn[3] * cur_meas[1] * dt; PHI[7][16] = -nav->C_bn[4] * cur_meas[2] * dt; PHI[7][17] = -nav->C_bn[5] * cur_meas[3] * dt; - PHI[8][15] = -nav->C_bn[6] * cur_meas[1] * dt; PHI[8][16] = -nav->C_bn[7] * cur_meas[2] * dt; PHI[8][17] = -nav->C_bn[8] * cur_meas[3] * dt; - - } - - TransposeMatrix(&PHI[0][0], nStates, nStates, &PHI_T[0][0]); - MultiplyMatrix(&PHI[0][0], &Pp[0][0], nStates, nStates, nStates, &PHI_P[0][0]); //PHI*P - MultiplyMatrix(&PHI_P[0][0], &PHI_T[0][0], nStates, nStates, nStates, &Pp[0][0]); //PHI*P*PHI_T - MultiplyMatrix(&PHI[0][0], &nav->Q[0][0], nStates, nStates, nStates, &tem_matrix[0][0]); //PHI*Q_ - MultiplyMatrix(&nav->Q[0][0], &PHI_T[0][0], nStates, nStates, nStates, &Q_w[0][0]); // Q_*PHI_T - - AddMatrix(&tem_matrix[0][0], &Q_w[0][0], nStates*nStates, &Q_w[0][0]); // PHI*Q_+Q_*PHI_T - MultiplyMatrixWithReal(0.5*dt, &Q_w[0][0], nStates*nStates, &Q_w[0][0]); //Q_w=0.5*(PHI*Q_+Q_*PHI_T)*dt - AddMatrix(&Pp[0][0], &Q_w[0][0], nStates*nStates, &Pp[0][0]); //P=PHI*P*PHI_T+Q_w - - for (i = 0; i < nStates; i++) - { - for (j = 0; j < nStates; j++) - { - nav->P[i][j] = Pp[i][j]; - } - } -} - -//get velocity by gyro data and the wheel radius -void getVel(double bufRawDa[][DaColNum], IMU_ConstPara *imu, S_NavState *nav) -{ - double Gx = 0.0; - double C_vn_angle[3] = { 0.0 }; - - for (int i = -Half_GmeanLen; i <= Half_GmeanLen; i++) - { - Gx += bufRawDa[HisLen + i][1]; - } - Gx /= GmeanLen; - - if (imu->IfcompV)//Compensate the gyro errors for Vv or not - { - nav->Gx_comped = (1 - nav->Sg[0])*(Gx - nav->Bg[0]); - nav->SysVodo = - imu->Wheel_Radius * nav->Gx_comped; - } - else - { - nav->SysVodo = -imu->Wheel_Radius * (Gx); - } - - if (imu->MountPos != 0) - { - C_vn_angle[2] = nav->Att[2] - PI / 2.0; - } - - Mat3 C_nv; - euler2rotation(nav->C_vn, C_vn_angle); - TransposeMatrix(nav->C_vn, 3, 3, C_nv); - MultiplyMatrix(C_nv,nav->C_bn, 3, 3, 3, nav->C_bv); - -} - -void States_Feedback(IMU_ConstPara *imu, S_NavState *nav) -{ - nav->Pos[0] = nav->Pos[0] - nav->X[0]; - nav->Pos[1] = nav->Pos[1] - nav->X[1]; - nav->Pos[2] = nav->Pos[2] - nav->X[2]; - - nav->Vel[0] -= nav->X[3]; - nav->Vel[1] -= nav->X[4]; - nav->Vel[2] -= nav->X[5]; - - double nPhi[3] = { 0.0 }; - double Qx[4] = { 0.0 }; - double Phi[3] = { nav->X[6], nav->X[7], nav->X[8] }; - double Phi_mag = sqrt(Phi[0] * Phi[0] + Phi[1] * Phi[1] + Phi[2] * Phi[2]); - double n = sin(0.5*Phi_mag) / Phi_mag; - MultiplyMatrixWithReal(n, Phi, 3, nPhi); - Qx[3] = cos(0.5*Phi_mag); - Qx[0] = nPhi[0]; - Qx[1] = nPhi[1]; - Qx[2] = nPhi[2]; - double Qx_inv[4], Qbn_new[4], deltaCbn[9]; - double Phi_skew[9] = { 0, -Phi[2], Phi[1], Phi[2], 0, -Phi[0], -Phi[1], Phi[0], 0 }; - - Quat_norm(Qx); - CopyMatrix(4, 1, Qx_inv, Qx); - quat2rotation(deltaCbn, Qx_inv); - - QuatMul(Qx_inv, nav->Q_bn, Qbn_new); - Quat_norm(Qbn_new); - CopyMatrix(3, 3, nav->Q_bn, Qbn_new); - quat2euler(nav->Att, Qbn_new); - quat2rotation(nav->C_bn, Qbn_new); - - - nav->Bg[0] += nav->X[9]; - nav->Bg[1] += nav->X[10]; - nav->Bg[2] += nav->X[11]; - - nav->Ba[0] += nav->X[12]; - nav->Ba[1] += nav->X[13]; - nav->Ba[2] += nav->X[14]; - - - - nav->Sg[0] += nav->X[15]; - nav->Sg[1] += nav->X[16]; - nav->Sg[2] += nav->X[17]; - - nav->Sa[0] += nav->X[18]; - nav->Sa[1] += nav->X[19]; - nav->Sa[2] += nav->X[20]; - - zero_array(nStates, nav->X); - - nav->FilterUpdate = 0; -} - - - -void EKF_Update(S_NavState *nav, int nMeas, double inno[], double H[], double R[]) -{ - - int mLen_1 = nStates * nMeas; - int mLen_2 = nStates * nStates; - int mLen_3 = nMeas * nMeas; - double * Pp = (double *)malloc(mLen_2 * sizeof(double)); - int i = 0, j = 0; - for (i = 0; i < nStates; i++) - { - for (j = 0; j < nStates; j++) - { - Pp[i*nStates + j] = nav->P[i][j]; - } - - } - - double * tem_matrix1 = (double *)malloc(mLen_1 * sizeof(double)); - double * tem_matrix2 = (double *)malloc(mLen_1 * sizeof(double)); - double * tem_matrix3 = (double *)malloc(mLen_1 * sizeof(double)); - - TransposeMatrix(H, nMeas, nStates, tem_matrix1);//Ht - MultiplyMatrix(Pp, tem_matrix1, nStates, nStates, nMeas, tem_matrix2); //P*Ht - - double * HPHTR = (double *)malloc(mLen_3 * sizeof(double)); - MultiplyMatrix(H, tem_matrix2, nMeas, nStates, nMeas, HPHTR); //H*P*Ht - AddMatrix(HPHTR, R, mLen_3, HPHTR);//H*P*Ht+R - double * U = (double *)malloc(mLen_3 * sizeof(double)); - CopyMatrix(nMeas, nMeas, U, HPHTR); - int pdf = Cholesky(U, nMeas); - if (pdf > 0) - { - double * invHPHTR = (double *)malloc(mLen_3 * sizeof(double)); - MatrixInv(nMeas, HPHTR, invHPHTR);//inv(H*P*Ht+R) - double * K = (double *)malloc(mLen_1 * sizeof(double)); - MultiplyMatrix(tem_matrix2, invHPHTR, nStates, nMeas, nMeas, K); - - double * dx = (double *)malloc(nStates * sizeof(double)); - MultiplyMatrix(K, inno, nStates, nMeas, 1, dx);//dx - - - double * I = (double *)malloc(mLen_2 * sizeof(double)); - UnitMatrix(nStates, I);//I - double * IKH = (double *)malloc(mLen_2 * sizeof(double)); - double * IKHP = (double *)malloc(mLen_2 * sizeof(double)); - double * tem_matrix4 = (double *)malloc(mLen_2 * sizeof(double)); - MultiplyMatrix(K, H, nStates, nMeas, nStates, IKH); // K*H - SubtractMatrix(I, IKH, mLen_2, IKH); // I-K*H - MultiplyMatrix(IKH, Pp, nStates, nStates, nStates, IKHP); //(I-K*H)*P - TransposeMatrix(IKH, nStates, nStates, tem_matrix4);//(I-K*H)_ - MultiplyMatrix(IKHP, tem_matrix4, nStates, nStates, nStates, Pp); //(I-K*H)*P*(I-K*H)_ - MultiplyMatrix(K, R, nStates, nMeas, nMeas, tem_matrix1); //K*R - TransposeMatrix(K, nStates, nMeas, tem_matrix2);//KT - MultiplyMatrix(tem_matrix1, tem_matrix2, nStates, nMeas, nStates, tem_matrix4); //K*R*K_T - AddMatrix(Pp, tem_matrix4, mLen_2, Pp); //P=(I+K*H)*P*(I+K*H)_+K*R*K_T - for (i = 0; i < nStates; i++) - { - for (j = 0; j < nStates; j++) - { - nav->P[i][j] = Pp[i*nStates + j]; - } - - } - - nav->FilterUpdate += nMeas; - - free(I); - free(IKH); - free(IKHP); - free(tem_matrix4); - - AddMatrix(dx, nav->X, nStates, nav->X); - - free(invHPHTR); - free(K); - free(dx); - - } - - free(Pp); - free(tem_matrix1); - free(tem_matrix2); - free(tem_matrix3); - free(U); - free(HPHTR); -} - - -void WriteFile(S_NavState Nav, IMU_ConstPara IMU, FILE *fp_out, FILE *fp_OutCov) -{ - - if (fp_out != NULL) - { - fwrite(&Nav.T_Cur, sizeof(double), 1, fp_out); - fwrite(Nav.Pos, sizeof(double), 3, fp_out); - fwrite(Nav.Vel, sizeof(double), 3, fp_out); - - double euler[3] = { 0.0 }; - euler[0] = Nav.Att[0]; - euler[1] = Nav.Att[1]; - euler[2] = Nav.Att[2]; - - if (IMU.MountPos != 0) - { - euler[2] -= PI / 2;//Transform to vehicle heading - ConvertPI(&euler[2]); - } - fwrite(euler, sizeof(double), 3, fp_out); - - fwrite(Nav.Bg, sizeof(double), 3, fp_out); - fwrite(Nav.Ba, sizeof(double), 3, fp_out); - fwrite(Nav.Sg, sizeof(double), 3, fp_out); - fwrite(Nav.Sa, sizeof(double), 3, fp_out); - fwrite(&Nav.SysVodo, sizeof(double), 1, fp_out); - - } - - if (fp_OutCov != NULL) - { - fwrite(&Nav.T_Cur, sizeof(double), 1, fp_OutCov); - for (int i = 0; i < nStates; i++) - { - double Out_P = sqrt(Nav.P[i][i]); - fwrite(&Out_P, sizeof(double), 1, fp_OutCov); - } - } - -} - -void OpenFile(IMU_ConstPara imu, FILE **fp_IMU, FILE **fp_out, FILE **fp_OutCov, int sysnum) -{ - char* FilePath = const_cast(imu.filepath.c_str()); - char* FileName = const_cast(imu.filename.c_str()); - - //open IMU file - char Fname_IMU[300] = { "" }; - strcpy_s(Fname_IMU, FilePath); - strcat_s(Fname_IMU, FileName); - fopen_s(fp_IMU, Fname_IMU, "rb"); - - //create results file - char optfolder[300] = { "" }; - strcpy_s(optfolder, FilePath); - strcat_s(optfolder, "output\\"); - if (_access(optfolder, 0) == -1) - _mkdir(optfolder); - - char NAVfile[300] = { "" }; - char Covfile[300] = { "" }; - char DRfile[300] = { "" }; - - strcpy_s(NAVfile, optfolder); - strcpy_s(Covfile, optfolder); - - std::string s = std::to_string(nStates); - strcat_s(NAVfile, s.c_str()); - strcat_s(Covfile, s.c_str()); - - if (sysnum == 1) - strcat_s(NAVfile, "-S-NAV.bin"); - - if (sysnum == 1) - strcat_s(Covfile, "-S-Cov.bin"); - - fopen_s(fp_out, NAVfile, "wb"); - fopen_s(fp_OutCov, Covfile, "wb"); - -} - -void DataBuff(int* RawDa_Sum, double* NowMeas, double BufRawDa[][DaColNum]) -{ - if (*RawDa_Sum < BufLen) - { - for (int i = 0; i < DaColNum; i++) - { - BufRawDa[(*RawDa_Sum)][i] = NowMeas[i]; - } - (*RawDa_Sum)++; - } - else - { - for (int i = 0; i < BufLen - 1; i++) - { - for (int j = 0; j < DaColNum; j++) - { - BufRawDa[i][j] = BufRawDa[i + 1][j]; - } - } - for (int j = 0; j < DaColNum; j++) - { - BufRawDa[BufLen - 1][j] = NowMeas[j]; - } - } -} - -int charfind(char *buf, char *sub) -{ - - int len = strlen(buf); - char *p = (char *)malloc(len * 2 + 1); - - memset(p, 0x00, len * 2 + 1); - - strcpy(p, buf); - strcat(p, buf); - - if (strstr(p, sub) == NULL) - { - return 0; - } - else - { - return 1; - } -} - -void NHC_Odo_Update(IMU_ConstPara *imu, S_NavState *nav) -{ - if (imu->Odo_IfUse == 1) - { - if ((nav->T_Cur - nav->T_Odo) > (imu->Odo_dt - Half_dt)) - { - double V_vel[3] = { 0.0 }; - double C_nv[9] = { 0.0 }; - - TransposeMatrix(nav->C_vn, 3, 3, C_nv);//current navigation result - MultiplyMatrix(C_nv, nav->Vel, 3, 3, 1, V_vel);//C_bv*C_nb*v_IMUn - - Vec3 tmp2; - rotation2euler(tmp2, nav->C_vn); - - - double vel_skew[9] = { 0, -nav->Vel[2], nav->Vel[1], nav->Vel[2], 0, -nav->Vel[0], -nav->Vel[1], nav->Vel[0], 0 }; - double V_velskew[9] = { 0.0 }; - MultiplyMatrix(C_nv, vel_skew, 3, 3, 3, V_velskew);//C_bv*C_nb*skewmatrix(V_IMUn) - - double CurGyros[3] = { nav->PreMeas[0] / nav->dt,nav->PreMeas[1] / nav->dt,nav->PreMeas[2] / nav->dt };//It was the increment when saved - double wib_skew[9] = { 0, -CurGyros[2], CurGyros[1], CurGyros[2], 0, -CurGyros[0], -CurGyros[1], CurGyros[0], 0 }; - - - double Cbvwskew[9] = { 0.0 }; - double cLeverArm_Skew[9] = { 0.0 }; - double vLeverArm[3] = { 0.0 }; - MultiplyMatrix(nav->C_bv, wib_skew, 3, 3, 3, Cbvwskew);//C_bv*skewmatrix(w_ibb) - MultiplyMatrix(Cbvwskew, imu->WheelLeverArm, 3, 3, 1, vLeverArm);//C_bv*skewmatrix(w_ibb)*LeverArm - - double Cbnwskew[9] = { 0.0 }; - double vnLeverArm[3] = { 0.0 }; - - MultiplyMatrix(nav->C_bn, wib_skew, 3, 3, 3, Cbnwskew); - MultiplyMatrix(Cbnwskew, imu->WheelLeverArm, 3, 3, 1, vnLeverArm);//C_bn*skewmatrix(w_ibb)*LeverArm - - double vnLeverArm_skew[9] = { 0, -vnLeverArm[2], vnLeverArm[1], vnLeverArm[2], 0, -vnLeverArm[0], -vnLeverArm[1], vnLeverArm[0], 0 }; - double vVLeverArm_skew[9] = { 0.0 }; - MultiplyMatrix(C_nv, vnLeverArm_skew, 3, 3, 3, vVLeverArm_skew);//C_nv (cross product) skewmatrix(C_bn*skewmatrix(w_ibb)*LA) - - double LeverArm_Skew[9] = { 0, -imu->WheelLeverArm[2], imu->WheelLeverArm[1], imu->WheelLeverArm[2], 0, -imu->WheelLeverArm[0], - -imu->WheelLeverArm[1], imu->WheelLeverArm[0], 0 }; - MultiplyMatrix(nav->C_bv, LeverArm_Skew, 3, 3, 3, cLeverArm_Skew);//C_bv*skewmatrix(LeverArm) - - double Cbv_LAskew_wibb[9] = { 0.0 }; - double diagwibb[9] = { CurGyros[0], 0.0, 0.0, 0.0, CurGyros[1], 0.0, 0.0, 0.0, CurGyros[2] }; - MultiplyMatrix(cLeverArm_Skew, diagwibb, 3, 3, 3, Cbv_LAskew_wibb); - - double Hv[3][nStates] = { 0.0 }; - for (int i = 0; i < 3; i++) - { - for (int j = 0; j < 3; j++) - { - Hv[i][j + 3] = C_nv[i * 3 + j]; - Hv[i][j + 6] = vVLeverArm_skew[i * 3 + j]; - Hv[i][j + 9] = -cLeverArm_Skew[i * 3 + j]; - Hv[i][j + 15] = -Cbv_LAskew_wibb[i * 3 + j]; - } - } - - Hv[0][8] = -V_velskew[2]; - Hv[1][8] = -V_velskew[5]; - Hv[2][8] = -V_velskew[8]; - - if (imu->IfcompV) - { - Hv[0][9] += -imu->Wheel_Radius; - Hv[1][10] += -imu->Wheel_Radius; - Hv[2][11] += -imu->Wheel_Radius; - - Hv[0][15] += -imu->Wheel_Radius * CurGyros[0]; - Hv[1][16] += -imu->Wheel_Radius * CurGyros[1]; - Hv[2][17] += -imu->Wheel_Radius * CurGyros[2]; - } - - - double Zv[3] = { 0.0 }; - Zv[0] = V_vel[0] + vLeverArm[0] - nav->SysVodo; - Zv[1] = V_vel[1] + vLeverArm[1]; - Zv[2] = V_vel[2] + vLeverArm[2]; - - - double Rv[9] = { 0.0 }; - Rv[0] = imu->Odo_Var[0]; - Rv[4] = imu->Odo_Var[1]; - Rv[8] = imu->Odo_Var[2]; - - //inno - double inno[3] = { 0.0 }; - double Hv_x[3] = { 0.0 }; - MultiplyMatrix(&Hv[0][0], nav->X, 3, nStates, 1, Hv_x); - SubtractMatrix(Zv, Hv_x, 3, inno); - - EKF_Update(nav, 3, inno, &Hv[0][0], Rv); - - nav->T_Odo = nav->T_Cur; - } - } -} diff --git a/Wheel-INS/NavFunc.h b/Wheel-INS/NavFunc.h deleted file mode 100644 index 0319a71..0000000 --- a/Wheel-INS/NavFunc.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef _NAV_STATE_ -#define _NAV_STATE_ - -#include -#include -#include -#include -#include -#include "DataType.h" -#include "Const.h" - -void One_Sys_Main(cv::FileStorage fs); - -//Auxiliary functions -double vec_norm(int len, double *da); - -void zero_array(int row, double *da); -int Read_oneEpoch(FILE *fp, double da[]); -void Configure_IMU_Para(IMU_ConstPara * IMU, cv::FileStorage fs); -void WriteFile(S_NavState Nav, IMU_ConstPara IMU, FILE *fp_out, FILE *fp_OutCov); -void OpenFile(IMU_ConstPara imu, FILE **fp_IMU, FILE **fp_out, FILE **fp_OutCov, int sysnum); -int charfind(char *buf, char *sub); -void DataBuff(int* RawDa_Sum, double* NowMeas, double BufRawDa[][DaColNum]); - - -//INS Mechanization -void ConvertPI(double* angle);//[0, 2pi] -> [-pi, pi] -void CrossProduct(Vec3 a, Vec3 b, Vec3 c); -void euler2rotation(Mat3 rotation, Vec3 euler);//euler in rad -void euler2quat(QuatVec quat, Vec3 euler); -void quat2rotation(Mat3 rotation, QuatVec quat); -void quat2euler(Vec3 euler, QuatVec quat); -void rotation2euler(Vec3 euler, Mat3 rotation); -void rotation2quat(QuatVec quat, Mat3 rotation); -void Bias_Compensate(double cur_meas[], IMU_ConstPara *imu, S_NavState *nav); -void INS_Mech(double pre_meas[], double cur_meas[], S_NavState *nav); - - -//EKF -void Initial_Nav(double stime, double cur_meas[], IMU_ConstPara *imu, S_NavState *nav); -void getVel(double bufRawDa[][DaColNum], IMU_ConstPara *imu, S_NavState *nav); - -void States_Feedback(IMU_ConstPara *imu, S_NavState *nav); -void NHC_Odo_Update(IMU_ConstPara *imu, S_NavState *nav); -void EKF_Predict(double cur_meas[], IMU_ConstPara *imu, S_NavState *nav); -void EKF_Update(S_NavState *nav, int nMeas, double inno[], double H[], double R[]); - - -//Matrix operations -void AddMatrix(double matrix_a[], double matrix_b[], int matrix_length, double result_matrix[]); -void SubtractMatrix(double matrix_a[], double matrix_b[], int matrix_length, double result_matrix[]); -void MultiplyMatrixWithReal(double real_number, double matrix[], int matrix_length, double result_matrix[]); -void MultiplyMatrix(double matrix_a[], double matrix_b[], int matrix_a_row, int matrix_a_column, int matrix_b_column, double result_matrix[]); -int Cholesky(double matrix[], int matrix_row); -void TransposeMatrix(double matrix[], int matrix_row, int matrix_column, double transpose_matrix[]); -void UnitMatrix(int matrix_order, double unit_matrix[]); -void ZeroMatrix(int matrix_row, int matrix_colunm, double zero_matrix[]); -void CopyMatrix(int row, int col, double matrix_result[], double matrix_b[]); -int MatrixInv(int n, double a[], double resault[]); -void diag_matrix(int row, double value, double *da); -bool QuatMul(double Q1[], double Q2[], double result_Q[]); -void Quatinv(double Q[], double Qinv[]); -bool Quat_norm(double Qbn[]); - -#endif diff --git a/Wheel-INS/OneSys.cpp b/Wheel-INS/OneSys.cpp deleted file mode 100644 index 8c23ba8..0000000 --- a/Wheel-INS/OneSys.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include -#include "NavFunc.h" - -void One_Sys_Main(cv::FileStorage fs) -{ - - IMU_ConstPara IMU; - Configure_IMU_Para(&IMU, fs); - - double sTime = (double)fs["start_t"]; - double eTime = (double)fs["end_t"]; - - //IMU data - FILE *fp_IMU = NULL; - //Output navigation results - FILE *fp_out = NULL; - //Output covariance - FILE *fp_OutCov = NULL; - - OpenFile(IMU, &fp_IMU, &fp_out, &fp_OutCov, 1); - - //IMU data buffer - double BufRawDa[BufLen][DaColNum]; - - //Init navigation struct - S_NavState Nav; - Nav.InitSum = 0; - Nav.InitFlag = 0; - - - Vec7 NowMeas; - Vec7 PreMeas; - Vec7 CurMeas; - - double dt = 0.0; - - int RawDa_Sum = 0; - bool initial_flag = 0; - - while (!feof(fp_IMU)) - { - Read_oneEpoch(fp_IMU, NowMeas); - DataBuff(&RawDa_Sum, NowMeas, BufRawDa); - if (RawDa_Sum == BufLen) - { - if (Nav.InitFlag == 0) - { - CopyMatrix(1, DaColNum, PreMeas, BufRawDa[HisLen - 1]); - CopyMatrix(1, DaColNum, CurMeas, BufRawDa[HisLen]); - if (CurMeas[0] >= sTime) - { - Initial_Nav(sTime, CurMeas, &IMU, &Nav); - } - } - else if (Nav.InitFlag == 1) - { - if (CurMeas[0] > eTime) - break; - - - CopyMatrix(1, DaColNum, PreMeas, CurMeas); - CopyMatrix(1, DaColNum, CurMeas, BufRawDa[HisLen]); - - Nav.T_Cur = CurMeas[0]; - Nav.dt = CurMeas[0] - PreMeas[0]; - - Bias_Compensate(CurMeas, &IMU, &Nav); - - //The INS Mechanization algorithm adopted in this system is a simplified version. Because we focused on MEMS IMU and local robot positioning applications, we ignored the earth rotation, the change of naviagtion frame and gravity, which are important for high-end IMU and large-scale applications. - //reference: E.-H. Shin, “Estimation techniques for low-cost inertial navigation,” Ph.D. dissertation, Dept. Geomatics Eng., Univ. of Calgary, Calgary, Canada, 2005. - INS_Mech(PreMeas, CurMeas, &Nav); - - EKF_Predict(CurMeas, &IMU, &Nav); - - getVel(BufRawDa, &IMU, &Nav); - - NHC_Odo_Update(&IMU, &Nav); - - if (Nav.FilterUpdate >= 1) - { - States_Feedback(&IMU, &Nav); - } - - WriteFile(Nav, IMU, fp_out, fp_OutCov); - } - } - } - fclose(fp_out); - fclose(fp_OutCov); -} - diff --git a/Wheel-INS/cMatrix.cpp b/Wheel-INS/cMatrix.cpp deleted file mode 100644 index 42187bf..0000000 --- a/Wheel-INS/cMatrix.cpp +++ /dev/null @@ -1,479 +0,0 @@ -#include "NavFunc.h" - -void ConvertPI(double* angle) -{ - //[0, 2pi] -> [-pi, pi] - if (*angle > PI) - { - *angle -= 2 * PI; - } - else if (*angle < -PI) - { - *angle += 2 * PI; - } -} - -void CrossProduct(Vec3 a, Vec3 b, Vec3 c) -{ - c[0] = a[1] * b[2] - a[2] * b[1]; - c[1] = a[2] * b[0] - a[0] * b[2]; - c[2] = a[0] * b[1] - a[1] * b[0]; -} - -void euler2rotation(Mat3 rotation, Vec3 euler) -{ - - double sin_phi = sin(euler[0]); - double cos_phi = cos(euler[0]); - double sin_theta = sin(euler[1]); - double cos_theta = cos(euler[1]); - double sin_psi = sin(euler[2]); - double cos_psi = cos(euler[2]); - - rotation[0] = cos_psi*cos_theta; - rotation[3] = sin_psi*cos_theta; - rotation[6] = -sin_theta; - rotation[1] = (-sin_psi*cos_phi) + cos_psi*(sin_theta*sin_phi); - rotation[4] = (cos_psi*cos_phi) + sin_psi*(sin_theta*sin_phi); - rotation[7] = cos_theta*sin_phi; - rotation[2] = (sin_psi*sin_phi) + cos_psi*(sin_theta*cos_phi); - rotation[5] = (-cos_psi*sin_phi) + sin_psi*(sin_theta*cos_phi); - rotation[8] = cos_theta*cos_phi; -} - -void rotation2quat(QuatVec quat, Mat3 rotation) -{ - - int decide = -1; - double max_value = -999.0; - double UU[4] = { 1 + rotation[0] + rotation[4] + rotation[8], - 1 + rotation[0] - rotation[4] - rotation[8], - 1 - rotation[0] + rotation[4] - rotation[8], - 1 - rotation[0] - rotation[4] + rotation[8] }; - for (int i = 0; i < 4; i++) - { - if (max_valued) - { - d = p; - is[k] = i; - js[k] = j; - } - } - } - - if (d= 0; k--) - { - if (js[k] != k) - { - for (j = 0; j