From 46e02e8d5aa74b344764a84b31c6366e9167f23f Mon Sep 17 00:00:00 2001
From: ZhiQiang_Yang98 <565093967qq.com>
Date: Tue, 28 Mar 2023 14:16:38 +0800
Subject: [PATCH] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E5=8F=8C=E5=AD=90=E6=A0=B7?=
 =?UTF-8?q?=E5=81=87=E8=AE=BE?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .DS_Store                                     | Bin 10244 -> 10244 bytes
 Matlab/.DS_Store                              | Bin 6148 -> 8196 bytes
 TC_NAV_Zjut_post/Src/real_time/ins.c          |  14 +++++++++-----
 .../Testing/Temporary/LastTest.log            |   4 ++--
 4 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/.DS_Store b/.DS_Store
index 21f3fc665bb7f6cace445becdb434a99280f61d6..3e88774bbfb3bde7fc7329143cba2708bd22f49c 100644
GIT binary patch
delta 56
zcmZn(XbITxT8P8k)Ko{o(9mFVqL{;EW8oW<%Y;f%1UH`&vE|;(s_=(pGqWf&GXVLo
B5W4^X

delta 60
zcmZn(XbITxT8P8Q!c0fO(9CRdyqLpeW8oW<_Y0JBm|I!^1x-PMGLxeORW_dxvE|;(
NuJDUx^M4U$W&lk(6A}Ob

diff --git a/Matlab/.DS_Store b/Matlab/.DS_Store
index 0a55dd030c7172fcec7976072fb914448ef5f0a3..5bbd36ca66488e420ca0f44da2e321283cbd4f34 100644
GIT binary patch
delta 272
zcmZoMXmOBWU|?W$DortDU;r^WfEYvza8E20o2aKK$_J7M@)<mH@{^Nt@{>T4n*}+<
zST-wg^fE7I=ir#wz+wtg#=>C95YJ%9V2-R5sM`!Er}rNWfGi|+lWSNSILu8=brcK@
z4JNa&I#|MuC<`vi%gN762Py~J4s;gOXa+roDuzsk0#qv))=ze0G23j$a*=ss!+s_~
zW{@>Nk8lGCSCD6b4*t$OnP0?nvOf<82hc>2KN%*+^GuuU$R<4b0=xOfhCNIGmnJsI

delta 124
zcmZp1XfcprU|?W$DortDU=RQ@Ie-{Mvv5r;6q~50D9Qzr2aBaJ6fhJs6fwjz7;h|G
z&dkU)`Mr?V=5oP{%!}DMI0Tu23V}d?8%Vf<4A@xsoq009j3>xA1}2EnAln!=$Mei#
F1^~uS6L|mt

diff --git a/TC_NAV_Zjut_post/Src/real_time/ins.c b/TC_NAV_Zjut_post/Src/real_time/ins.c
index 2a994e1..bc67253 100644
--- a/TC_NAV_Zjut_post/Src/real_time/ins.c
+++ b/TC_NAV_Zjut_post/Src/real_time/ins.c
@@ -144,16 +144,20 @@ static void savepimu(ins_t *ins){
  *                        O    uipdates body-to-ecef coordinate transformation matrix
  *           double *omgb I    angular rate of body frame (rad/s) w.r.t eci-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
  * 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 Cbb[9]={1,0,0,0,1,0,0,0,1},Cei[9]={0};
     int i;
 //    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);
     matmul3("NN",Ca,Ca,Ca2);
     mag_alpha=norm(alpha,3);
@@ -273,7 +277,7 @@ extern void rotscull_corr(ins_t *ins,double dt,
 }
 
 /* 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->fbp   ,ins->fb  ,1,3);
@@ -352,7 +356,7 @@ extern int updateins(ins_t *ins,const imud_t *data){
 
     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]);
-    savepins(ins,data);
+    savepins(ins);
 
 //    dt= timediff(data->syn_time,ins->gtime);
     dt = 0.01;
@@ -381,7 +385,7 @@ extern int updateins(ins_t *ins,const imud_t *data){
 
     /* update attitude */
     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];
     skewsym3(alpha,Ca);
diff --git a/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log b/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log
index ef7c738..bf9b6d6 100644
--- a/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log
+++ b/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log
@@ -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