Wakanda_shaw/ZJUT_Nav
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

提交双子样假设

Browse Source
This commit is contained in:
ZhiQiang_Yang98 2023-03-28 14:16:38 +08:00
parent 380de59139
commit 46e02e8d5a
4 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

BIN
Matlab/.DS_Store vendored
View File

Binary file not shown.

14
TC_NAV_Zjut_post/Src/real_time/ins.c
View File

@ -144,16 +144,20 @@ static void savepimu(ins_t *ins){
* O uipdates body-to-ecef coordinate transformation matrix * O uipdates body-to-ecef coordinate transformation matrix
* double *omgb I angular rate of body frame (rad/s) w.r.t eci-frame * double *omgb I angular rate of body frame (rad/s) w.r.t eci-frame
* expressed in ecef-frame * expressed in ecef-frame
* double *omgbp I last angular rate of body frame (rad/s) w.r.t eci-frame
* expressed in ecef-frame
* double *das I rotational and sculling motion correction * double *das I rotational and sculling motion correction
* return :none * return :none
* ---------------------------------------------------------------------------*/ * ---------------------------------------------------------------------------*/
static void updateatt(double t, double *Cbe, const double *omgb,const double *das) static void updateatt(double t, double *Cbe, const double *omgb, const double *omgbp,const double *das)
{ {
double alpha[3],mag_alpha,a1,a2,Ca[9],Ca2[9],Cbep[9]; double alpha[3],mag_alpha,a1,a2,Ca[9],Ca2[9],Cbep[9];
double Cbb[9]={1,0,0,0,1,0,0,0,1},Cei[9]={0}; double Cbb[9]={1,0,0,0,1,0,0,0,1},Cei[9]={0};
int i; int i;
// trace(3,"updateatt:\n"); // trace(3,"updateatt:\n");
for (i=0;i<3;i++) alpha[i]=omgb[i]*t+das[i]; // for (i=0;i<3;i++) alpha[i]=omgb[i]*t+das[i];
//Twin-like hypothesis
for (i=0;i<3;i++) alpha[i]=omgb[i]*t+omgbp[i]*t*omgb[i]*t/12+das[i];
skewsym3(alpha,Ca); skewsym3(alpha,Ca);
matmul3("NN",Ca,Ca,Ca2); matmul3("NN",Ca,Ca,Ca2);
mag_alpha=norm(alpha,3); mag_alpha=norm(alpha,3);
@ -273,7 +277,7 @@ extern void rotscull_corr(ins_t *ins,double dt,
} }
/* save precious epoch ins states--------------------------------------------*/ /* save precious epoch ins states--------------------------------------------*/
static void savepins(ins_t *ins,const imud_t *data) static void savepins(ins_t *ins)
{ {
matcpy(ins->omgbp ,ins->omgb,1,3); matcpy(ins->omgbp ,ins->omgb,1,3);
matcpy(ins->fbp ,ins->fb ,1,3); matcpy(ins->fbp ,ins->fb ,1,3);
@ -352,7 +356,7 @@ extern int updateins(ins_t *ins,const imud_t *data){
traceins(3,ins); traceins(3,ins);
trace(3,"imu %lf,%lf,%lf,%lf,%lf,%lf\n",data->accl[0],data->accl[1],data->accl[2],data->gyro[0],data->gyro[1],data->gyro[2]); trace(3,"imu %lf,%lf,%lf,%lf,%lf,%lf\n",data->accl[0],data->accl[1],data->accl[2],data->gyro[0],data->gyro[1],data->gyro[2]);
savepins(ins,data); savepins(ins);
// dt= timediff(data->syn_time,ins->gtime); // dt= timediff(data->syn_time,ins->gtime);
dt = 0.01; dt = 0.01;
@ -381,7 +385,7 @@ extern int updateins(ins_t *ins,const imud_t *data){
/* update attitude */ /* update attitude */
for (int i=0;i<9;i++) Cbe[i]=ins->Cbe[i]; for (int i=0;i<9;i++) Cbe[i]=ins->Cbe[i];
updateatt(dt,ins->Cbe,ins->omgb,das); updateatt(dt,ins->Cbe,ins->omgb,ins->omgbp,das);
for (int i=0;i<3;i++) alpha[i]=ins->omgb[i]*dt+das[i]; for (int i=0;i<3;i++) alpha[i]=ins->omgb[i]*dt+das[i];
skewsym3(alpha,Ca); skewsym3(alpha,Ca);

4
TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log
View File

@ -1,3 +1,3 @@
Start testing: Mar 25 14:06 CST Start testing: Mar 27 20:51 CST
---------------------------------------------------------- ----------------------------------------------------------
End testing: Mar 25 14:06 CST End testing: Mar 27 20:51 CST