├── example.bag ├── pic └── screenshot.png ├── .idea ├── stim300.iml ├── vcs.xml ├── misc.xml ├── modules.xml └── workspace.xml ├── launch └── od_sins_realtime.launch ├── README.md ├── include ├── KFApp.h └── PSINS.h ├── CMakeLists.txt ├── package.xml └── src ├── KFApp.cpp ├── main.cpp └── PSINS.cpp /example.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BohemianRhapsodyz/PGO-INSAM/HEAD/example.bag -------------------------------------------------------------------------------- /pic/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BohemianRhapsodyz/PGO-INSAM/HEAD/pic/screenshot.png -------------------------------------------------------------------------------- /.idea/stim300.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /launch/od_sins_realtime.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PGO-INSAM 2 | 3 | ### Overview 4 | 5 | A Pose Graph Optimization version for Integrated Navitation System(GNSS/INS) based on GTSAM 6 | 7 | ### Dependency 8 | 9 | - [gtsam](https://github.com/borglab/gtsam/releases) (Georgia Tech Smoothing and Mapping library) 10 | ``` 11 | wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.2.zip 12 | cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/ 13 | cd ~/Downloads/gtsam-4.0.2/ 14 | mkdir build && cd build 15 | cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF .. 16 | sudo make install -j8 17 | ``` 18 | ### Usage 19 | ``` 20 | mkdir INSAM 21 | cd INSAM 22 | mkdir src 23 | cd src 24 | catkin_init_workspace 25 | git clone https://github.com/BohemianRhapsodyz/PGO-INSAM.git 26 | cd .. 27 | catkin_make 28 | source devel/setup.bash 29 | roslaunch stim300 od_sins_realtime.launch 30 | rosbag play example.bag 31 | ``` 32 | check rostopic /ground_truth and /nav to compare the result. 33 | ![Alt text](https://github.com/BohemianRhapsodyz/PGO-INSAM/blob/main/pic/screenshot.png) 34 | -------------------------------------------------------------------------------- /include/KFApp.h: -------------------------------------------------------------------------------- 1 | #ifndef _KFAPP_H 2 | #define _KFAPP_H 3 | 4 | #include "PSINS.h" 5 | class CSINSOD; 6 | 7 | typedef struct { 8 | CVect3 wm, vm, fog; 9 | double t; 10 | CVect3 gpsv, gpsp; 11 | } ImuGpsData; 12 | 13 | class CSINSOD :public CSINSTDKF // sizeof(CSINSGPSOD)~=30k bytes 14 | { 15 | public: 16 | CVect3 posDR, vnRes, posRes; 17 | CVect3 lvOD; 18 | CMat3 Cbo, MpkD; // Cbo: from body-frame to OD-frame 19 | double Kod, tODInt; 20 | BOOL measGPSvnValid, measGPSposValid, measODValid, measMAGyawValid; 21 | 22 | CSINSOD(void); 23 | virtual void Init(const CSINS &sins0, int grade = -1); 24 | virtual void SetFt(int nnq); 25 | virtual void SetMeas(void); 26 | virtual void Feedback(double fbts); 27 | void SetMeasGPS(const CVect3 &pgps = O31, const CVect3 &vgps = O31); 28 | void SetMeasOD(double dSod, double ts); 29 | void SetMeasYaw(double ymag); 30 | int Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts); 31 | int TDUpdate(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts, int nStep); 32 | }; 33 | 34 | //Initial pos and att 35 | double latitude0=0.59393781, longtitude0=1.8983, altitude0=444.84; 36 | double roll0=0.01843, pitch0=0.0171802, yaw0=3.064575; 37 | //double latitude0=0.393280276, longtitude0=1.988601613, altitude0=91.169; 38 | //double roll0=0.034417893, pitch0=0.0312763, yaw0=1.888446251; 39 | #endif 40 | 41 | 42 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(stim300) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-o3) 7 | 8 | ## Find catkin macros and libraries 9 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 10 | ## is used, also find other catkin packages 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | tf 16 | rosbag 17 | pcl_conversions 18 | ) 19 | 20 | find_package(cmake_modules REQUIRED) 21 | find_package(GTSAM REQUIRED QUIET) 22 | find_package(PCL REQUIRED QUIET) 23 | #find_package(Eigen3 REQUIRED) 24 | 25 | #find_package(PCL 1.7 REQUIRED) 26 | 27 | catkin_package( 28 | # INCLUDE_DIRS include 29 | # LIBRARIES lidar_imu_calib 30 | # CATKIN_DEPENDS roscpp rospy sensor_msgs 31 | # DEPENDS system_lib 32 | ) 33 | 34 | ## Specify additional locations of header files 35 | ## Your package locations should be listed before other locations 36 | include_directories( 37 | include 38 | ${catkin_INCLUDE_DIRS} 39 | ${GTSAM_INCLUDE_DIR} 40 | ${PCL_INCLUDE_DIRS} 41 | ) 42 | 43 | #include_directories(${EIGEN3_INCLUDE_DIR}) 44 | #add_definitions(${EIGEN3_DEFINITIONS}) 45 | link_directories( 46 | include 47 | ${GTSAM_LIBRARY_DIRS} 48 | ${PCL_LIBRARY_DIRS} 49 | ) 50 | #include_directories(${PCL_INCLUDE_DIRS}) 51 | #link_directories(${PCL_LIBRARY_DIRS}) 52 | #add_definitions(${PCL_DEFINITIONS}) 53 | 54 | add_library(cpsins 55 | src/PSINS.cpp 56 | src/KFApp.cpp 57 | ) 58 | 59 | target_link_libraries(cpsins ${catkin_LIBRARIES} ) 60 | 61 | add_executable(od_sins_node src/main.cpp) 62 | target_link_libraries(od_sins_node cpsins ${PCL_LIBRARIES} gtsam) 63 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | stim300 4 | 0.0.0 5 | The stim300 package 6 | 7 | 8 | 9 | 10 | hx 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/KFApp.cpp: -------------------------------------------------------------------------------- 1 | #include "KFApp.h" 2 | #include "PSINS.h" 3 | /*************************** class CSINSOD *********************************/ 4 | CSINSOD::CSINSOD(void) :CSINSTDKF(21, 10) 5 | { 6 | lvOD = O31; 7 | tODInt = 0.0; Kod = 1.0; 8 | Cbo = I33; MpkD = O33; 9 | Hk.SetMat3(6, 6, I33); Hk.SetMat3(6, 18, -I33); 10 | Hk(9, 2) = 1.0; 11 | measGPSvnValid = measGPSposValid = measODValid = measMAGyawValid = 0; 12 | } 13 | 14 | void CSINSOD::Init(const CSINS &sins0, int grade) 15 | { 16 | CSINSKF::Init(sins0); 17 | sins.lever(-lvOD); 18 | sins.pos = sins.posL; 19 | //posDR = sins0.pos; 20 | posDR = sins.posL; 21 | //posDR = sins.pos; 22 | 23 | Pmax.Set2(10.0*glv.deg, 10.0*glv.deg, 30.0*glv.deg, 24 | 50.0, 50.0, 50.0, 25 | 1.0e4 / glv.Re, 1.0e4 / glv.Re, 1.0e4, 26 | 10.0*glv.dph, 10.0*glv.dph, 10.0*glv.dph, 27 | 10.0*glv.mg, 10.0*glv.mg, 10.0*glv.mg, 28 | 5 * glv.deg, 0.1, 5 * glv.deg, 29 | 1.0e4 / glv.Re, 1.0e4 / glv.Re, 1.0e4); 30 | 31 | Pmin.Set2(0.01*glv.min, 0.01*glv.min, 0.1*glv.min, 32 | 0.01, 0.01, 0.1, 33 | 1.0 / glv.Re, 1.0 / glv.Re, 0.1, 34 | 0.001*glv.dph, 0.001*glv.dph, 0.001*glv.dph, 35 | 5.0*glv.ug, 5.0*glv.ug, 15.0*glv.ug, 36 | 1 * glv.min, 0.0001, 0.2*glv.min, 37 | 1.0 / glv.Re, 1.0 / glv.Re, 1.0); 38 | 39 | Pk.SetDiag2(1.0*glv.deg, 1.0*glv.deg, 1.0*glv.deg, 40 | 0.5, 0.5, 0.5, 41 | 5.0 / glv.Re, 5.0 / glv.Re, 10.0, 42 | 0.01*glv.dph, 0.01*glv.dph, 0.01*glv.dph, 43 | .10*glv.mg, .10*glv.mg, .10*glv.mg, 44 | 10.0*glv.min, 0.05, 15 * glv.min, 45 | 5 / glv.Re, 5 / glv.Re, 10); 46 | 47 | Qt.Set2(0.001*glv.dpsh, 0.001*glv.dpsh, 0.001*glv.dpsh, 48 | 1.0*glv.ugpsHz, 1.0*glv.ugpsHz, 1.0*glv.ugpsHz, 49 | 0.0, 0.0, 0.0, 50 | 0.0*glv.dphpsh, 0.0*glv.dphpsh, 0.0*glv.dphpsh, 51 | 0.0*glv.ugpsh, 0.0*glv.ugpsh, 0.0*glv.ugpsh, 52 | 0.0, 0.0, 0.0, 53 | 0.0, 0.0, 0.0); 54 | 55 | Xmax.Set(INF, INF, INF, 56 | INF, INF, INF, 57 | INF, INF, INF, 58 | 0.1*glv.dph, 0.1*glv.dph, 0.1*glv.dph, 59 | 200.0*glv.ug, 200.0*glv.ug, 200.0*glv.ug, 60 | 10 * glv.deg, 0.1, 10 * glv.deg, 61 | INF, INF, INF); 62 | 63 | Rt.Set2(0.2, 0.2, 0.6, 64 | 10.0 / glv.Re, 10.0 / glv.Re, 30.0, 65 | 0.5 / RE, 0.5 / RE, 2); 66 | 67 | Rmax = Rt * 100; Rmin = Rt * 0.01; Rb = 0.9; 68 | FBTau.Set(1.0, 1.0, 10.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0); 69 | // FBTau.Set(1.0, 1.0, 10.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, INF, INF, INF); 70 | } 71 | 72 | void CSINSOD::SetFt(int nnq) 73 | { 74 | CSINSKF::SetFt(15); 75 | CMat3 MvkD = norm(sins.vn)*CMat3(-sins.Cnb.e02, sins.Cnb.e01, sins.Cnb.e00, 76 | -sins.Cnb.e12, sins.Cnb.e11, sins.Cnb.e10, -sins.Cnb.e22, sins.Cnb.e21, sins.Cnb.e20); 77 | Ft.SetMat3(18, 0, sins.Mpv*askew(sins.vn)); 78 | Ft.SetMat3(18, 15, sins.Mpv*MvkD); 79 | Ft.SetMat3(18, 18, sins.Mpp); 80 | } 81 | 82 | void CSINSOD::SetMeas(void) 83 | { 84 | if (measGPSvnValid) { SetMeasFlag(000007); } // SINS/GPS_VEL 85 | if (measGPSposValid) { SetMeasFlag(000070); } // SINS/GPS_POS 86 | if (measODValid) { SetMeasFlag(000700); } // SINS/DR 87 | if (measMAGyawValid) { SetMeasFlag(001000); } // SINS/MAG_YAW 88 | measGPSvnValid = measGPSposValid = measODValid = measMAGyawValid = 0; 89 | } 90 | 91 | void CSINSOD::Feedback(double fbts) 92 | { 93 | CSINSKF::Feedback(fbts); 94 | Cbo = Cbo * a2mat(CVect3(FBXk.dd[15], 0.0, FBXk.dd[17])); 95 | Kod *= 1 - FBXk.dd[16]; 96 | posDR -= *(CVect3*)(&FBXk.dd[18]); 97 | } 98 | 99 | void CSINSOD::SetMeasGPS(const CVect3 &pgps, const CVect3 &vgps) 100 | { 101 | if (!IsZero(pgps, EPS)) 102 | { 103 | *(CVect3*)&Zk.dd[3] = sins.pos - pgps; 104 | measGPSposValid = 1; 105 | } 106 | if (!IsZero(vgps, EPS)) 107 | { 108 | *(CVect3*)&Zk.dd[0] = sins.vn - vgps; 109 | measGPSvnValid = 1; 110 | } 111 | } 112 | 113 | void CSINSOD::SetMeasOD(double dSod, double ts) 114 | { 115 | CVect3 dSn = sins.Cnb*(Cbo*CVect3(0, dSod*Kod, 0)); 116 | // CVect3 dSn = sins.Cnb*(CVect3(0, dSod*Kod, 0)); 117 | posDR = posDR + sins.eth.vn2dpos(dSn, 1.0); 118 | tODInt += ts; 119 | sins.lever(lvOD); 120 | //if (tODInt > 0.03) 121 | // { 122 | *(CVect3*)&Zk.dd[6] = sins.posL - posDR; 123 | tODInt = 0.0; 124 | measODValid = 1; 125 | // } 126 | //else 127 | //{ 128 | // tODInt = 0.0; 129 | //} 130 | //CVect3 dSn = sins.Cnb*(Cbo*CVect3(0, dSod*Kod, 0)); 131 | //posDR = posDR + sins.eth.vn2dpos(dSn, 1.0); 132 | //tODInt += ts; 133 | //if (fabs(sins.wnb.k) < .5*glv.dps) 134 | //{ 135 | // if (tODInt > 1.0) 136 | // { 137 | // *(CVect3*)&Zk.dd[6] = sins.pos - posDR; 138 | // tODInt = 0.0; 139 | // measODValid = 1; 140 | // } 141 | //} 142 | //else 143 | //{ 144 | // tODInt = 0.0; 145 | //} 146 | } 147 | 148 | void CSINSOD::SetMeasYaw(double ymag) 149 | { 150 | if (ymag > EPS || ymag < -EPS) // !IsZero(yawGPS) 151 | if (sins.att.i<30 * DEG && sins.att.i>-30 * DEG) 152 | { 153 | *(CVect3*)&Zk.dd[9] = -diffYaw(sins.att.k, ymag); 154 | measMAGyawValid = 1; 155 | } 156 | } 157 | 158 | int CSINSOD::Update(const CVect3 *pwm, const CVect3 *pvm, int nn, double ts) 159 | { 160 | int res = TDUpdate(pwm, pvm, nn, ts, 5); 161 | vnRes = sins.vn + sins.Cnb*askew(sins.web)*lvOD; 162 | posRes = sins.pos + sins.MpvCnb*lvOD; 163 | return res; 164 | } 165 | 166 | int CSINSOD::TDUpdate(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts, int nStep) 167 | { 168 | // if(sins.tk>99) 169 | // int debugi = 1; 170 | 171 | sins.Update(pwm, pvm, nSamples, ts); 172 | Feedback(sins.nts); 173 | for (int j = 0; j < nr; j++) measlost.dd[j] += sins.nts; 174 | 175 | measRes = 0; 176 | 177 | if (nStep <= 0 || nStep >= maxStep) { nStep = maxStep; } 178 | tdStep = nStep; 179 | 180 | tdts += sins.nts; kftk = sins.tk; 181 | meanfn = meanfn + sins.fn; ifn++; 182 | for (int i = 0; i < nStep; i++) 183 | { 184 | if (iter == -2) // -2: set measurements 185 | { 186 | if (ifn == 0) break; 187 | CVect3 vtmp = meanfn * (1.0 / ifn); meanfn = O31; ifn = 0; 188 | sins.fn = vtmp; SetFt(nq); sins.fn = vtmp; 189 | SetMeas(); SetHk(nq); 190 | } 191 | else if (iter == -1) // -1: discrete 192 | { 193 | Fk = ++(Ft*tdts); // Fk = I+Ft*ts 194 | Qk = Qt * tdts; 195 | Xk = Fk * Xk; 196 | // RtFading(tdts); 197 | meantdts = tdts; tdts = 0.0; 198 | } 199 | else if (iter < nq) // 0 -> (nq-1): Fk*Pk 200 | { 201 | int row = iter; 202 | RowMul(Pk1, Fk, Pk, row); 203 | } 204 | else if (iter < 2 * nq) // nq -> (2*nq-1): Fk*Pk*Fk+Qk 205 | { 206 | int row = iter - nq; 207 | RowMulT(Pk, Pk1, Fk, row); 208 | Pk.dd[nq*row + row] += Qk.dd[row]; 209 | // if(row==nq-1) { Pk += Qk; } 210 | } 211 | else if (iter < 2 * (nq + nr)) // (2*nq) -> (2*(nq+nr)-1): sequential measurement updating 212 | { 213 | if (measstop > 0) measflag = 0; 214 | int row = (iter - 2 * Ft.row) / 2; 215 | int flag = measflag & (0x01 << row); 216 | if (flag) 217 | { 218 | // if((iter-2*Ft.row)%2==0) 219 | if (iter % 2 == 0) 220 | { 221 | Hi = Hk.GetRow(row); 222 | Pxz = Pk * (~Hi); 223 | Pz0 = (Hi*Pxz)(0, 0); 224 | innovation = Zk(row) - (Hi*Xk)(0, 0); 225 | adptOKi = 1; 226 | if (Rb.dd[row] > EPS) 227 | adptOKi = RAdaptive(row, innovation, Pz0); 228 | double Pzz = Pz0 + Rt(row) / rts(row); 229 | Kk = Pxz * (1.0 / Pzz); 230 | } 231 | else 232 | { 233 | measflag ^= flag; 234 | if (adptOKi) 235 | { 236 | measRes |= flag; 237 | Xk += Kk * innovation; 238 | Pk -= Kk * (~Pxz); 239 | measlost.dd[row] = 0.0; 240 | } 241 | if (Zfd0.dd[row] < INF / 2) 242 | { 243 | RPkFading(row); 244 | } 245 | } 246 | } 247 | else 248 | { 249 | nStep++; 250 | } 251 | if (iter % 2 == 0) 252 | RtFading(row, meantdts); 253 | } 254 | else if (iter == 2 * (nq + nr)) // 2*(nq+nr): Xk,Pk constrain & symmetry 255 | { 256 | XPConstrain(); 257 | symmetry(Pk); 258 | } 259 | else if (iter >= 2 * (nq + nr) + 1) // 2*(nq+nr)+1: Miscellanous 260 | { 261 | Miscellanous(); 262 | iter = -3; 263 | } 264 | iter++; 265 | } 266 | SecretAttitude(); 267 | if (measstop > -1000000) measstop--; 268 | 269 | measflaglog |= measRes; 270 | return measRes; 271 | } 272 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "PSINS.h" 25 | #include "KFApp.h" 26 | 27 | typedef pcl::PointXYZI PointType; 28 | 29 | using namespace std; 30 | using namespace gtsam; 31 | 32 | using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) 33 | using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) 34 | using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) 35 | 36 | template 37 | double ROS_TIME(T msg) 38 | { 39 | return msg->header.stamp.toSec(); 40 | } 41 | 42 | 43 | class IMUPreintegration 44 | { 45 | public: 46 | 47 | ros::NodeHandle nh; 48 | 49 | ros::Subscriber subImu; 50 | ros::Subscriber subOdometry; 51 | ros::Publisher pubImuOdometry; 52 | 53 | bool systemInitialized = false; 54 | bool gpsInitialized = false; 55 | 56 | gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; 57 | gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise; 58 | gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise; 59 | gtsam::noiseModel::Diagonal::shared_ptr correctionNoise; 60 | gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2; 61 | gtsam::Vector noiseModelBetweenBias; 62 | 63 | 64 | gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_; 65 | gtsam::PreintegratedImuMeasurements *imuIntegratorImu_; 66 | gtsam::PreintegratedImuMeasurements *imuIntegratorImu_1; 67 | 68 | std::deque imuQueOpt; 69 | std::deque imuQueImu; 70 | 71 | CVect3 gps0; 72 | std::deque gpsQueue; 73 | 74 | gtsam::Pose3 prevPose_; 75 | gtsam::Vector3 prevVel_; 76 | gtsam::NavState prevState_; 77 | gtsam::imuBias::ConstantBias prevBias_; 78 | 79 | gtsam::NavState prevStateOdom; 80 | gtsam::imuBias::ConstantBias prevBiasOdom; 81 | 82 | float imuAccNoise=0.01, imuGyrNoise=0.001, imuAccBiasN=0.0002, imuGyrBiasN=0.00003; 83 | 84 | bool doneFirstOpt = false; 85 | double lastImuT_imu = -1; 86 | double lastImuT_opt = -1; 87 | 88 | gtsam::ISAM2 optimizer; 89 | gtsam::NonlinearFactorGraph graphFactors; 90 | gtsam::Values graphValues; 91 | 92 | const double delta_t = 0; 93 | 94 | int key = 1; 95 | 96 | //geometry_msgs::Quaternion qimu2lidar = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -PI/2); 97 | geometry_msgs::Quaternion qimu2lidar = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0); 98 | 99 | //gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(qimu2lidar.w, qimu2lidar.x, qimu2lidar.y, qimu2lidar.z), gtsam::Point3(0, 0, 0)); 100 | gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(0, 0, 0)); 101 | gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(0, 0, 0)); 102 | 103 | IMUPreintegration() 104 | { 105 | subImu = nh.subscribe ("stim300", 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay()); 106 | subOdometry = nh.subscribe("novtel", 2000, &IMUPreintegration::gpsHandler, this, ros::TransportHints().tcpNoDelay()); 107 | //subImu = nh.subscribe ("/udi/imu_s/data", 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay()); 108 | //subOdometry = nh.subscribe("/novatel718d/pos", 2000, &IMUPreintegration::gpsHandler, this, ros::TransportHints().tcpNoDelay()); 109 | 110 | pubImuOdometry = nh.advertise ("/nav", 2000); 111 | 112 | boost::shared_ptr p = gtsam::PreintegrationParams::MakeSharedU(9.80511); 113 | p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous 114 | p->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous 115 | p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities 116 | gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias 117 | 118 | priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m 119 | priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s 120 | priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good 121 | correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m 122 | correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m 123 | noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished(); 124 | 125 | imuIntegratorImu_1 = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); 126 | imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread 127 | imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization 128 | } 129 | 130 | CVect3 lla2enu(double latitude, double longtitude, double altitude, double latitude0, double longtitude0, double altitude0) 131 | { 132 | double Re=6378137; 133 | double f=1/298.257; 134 | double pi=3.14159265359; 135 | double deg2rad, e, e2, s1, c1, s12, sq, sq2, RMh, RNh, c1RNh; 136 | double tmp_latitude0, tmp_longtitude0; 137 | double x,y,z; 138 | deg2rad=pi/180; 139 | tmp_latitude0=latitude0; 140 | tmp_longtitude0=longtitude0; 141 | e=sqrt(2*f-f*f); 142 | e2=e*e; 143 | s1=sin(latitude); 144 | c1=cos(latitude); 145 | s12=s1*s1; 146 | sq=1-e2*s12; 147 | sq2=sqrt(sq); 148 | RMh=Re*(1-e2)/sq/sq2+altitude; 149 | RNh=Re/sq2+altitude; 150 | c1RNh=c1*RNh; 151 | x=(longtitude-tmp_longtitude0)*c1RNh; 152 | y=(latitude-tmp_latitude0)*RMh; 153 | z=altitude-altitude0; 154 | return CVect3(x,y,z); 155 | } 156 | 157 | void resetOptimization() 158 | { 159 | gtsam::ISAM2Params optParameters; 160 | optParameters.relinearizeThreshold = 0.1; 161 | optParameters.relinearizeSkip = 1; 162 | optimizer = gtsam::ISAM2(optParameters); 163 | 164 | gtsam::NonlinearFactorGraph newGraphFactors; 165 | graphFactors = newGraphFactors; 166 | 167 | gtsam::Values NewGraphValues; 168 | graphValues = NewGraphValues; 169 | } 170 | 171 | void resetParams() 172 | { 173 | lastImuT_imu = -1; 174 | doneFirstOpt = false; 175 | systemInitialized = false; 176 | } 177 | 178 | void gpsHandler(const sensor_msgs::NavSatFix::ConstPtr& gpsMsg) 179 | { 180 | double currentCorrectionTime = ROS_TIME(gpsMsg); 181 | 182 | if (imuQueOpt.empty()) 183 | return; 184 | 185 | //CVect3 enu=lla2enu(PI/180*gpsMsg->latitude, PI/180*gpsMsg->longitude, gpsMsg->altitude, latitude0, longtitude0, altitude0); 186 | CVect3 enu=lla2enu(gpsMsg->latitude, gpsMsg->longitude, gpsMsg->altitude, latitude0, longtitude0, altitude0); 187 | nav_msgs::Odometry gpsodom; 188 | gpsodom.header.stamp = gpsMsg->header.stamp; 189 | gpsodom.pose.pose.position.x = enu.i; 190 | gpsodom.pose.pose.position.y = enu.j; 191 | gpsodom.pose.pose.position.z = enu.k; 192 | //gpsQueue.push_back(gpsodom); 193 | 194 | float p_x = enu.i; 195 | float p_y = enu.j; 196 | float p_z = enu.k; 197 | 198 | //quat convert 199 | Eigen::Vector3d extV(PI/2,0,0); 200 | Eigen::Matrix3d extRPY1, extRPY2; 201 | Eigen::Quaterniond extQRPY1, extQRPY2; 202 | extRPY1 << 1, 0, 0, 203 | 0,-1, 0, 204 | 0, 0, 1; 205 | extQRPY1 = Eigen::Quaterniond(extRPY1); 206 | // extRPY2 = Eigen::AngleAxisd(extV[0], Eigen::Vector3d::UnitZ()) 207 | // *Eigen::AngleAxisd(extV[1], Eigen::Vector3d::UnitY()) 208 | // *Eigen::AngleAxisd(extV[2], Eigen::Vector3d::UnitX()); 209 | extRPY2 = Eigen::AngleAxisd(extV[0], Eigen::Vector3d::UnitZ()); 210 | //extQRPY2 = Eigen::Quaterniond(extRPY2); 211 | extQRPY2 = extRPY2; 212 | 213 | //CQuat(CVect3(roll0,pitch0,yaw0)); 214 | //geometry_msgs::Quaternion q0 = tf::createQuaternionMsgFromRollPitchYaw(roll0, -pitch0, (yaw0+PI/2)); 215 | geometry_msgs::Quaternion q0 = tf::createQuaternionMsgFromRollPitchYaw(-roll0, pitch0, (yaw0+PI/2)); 216 | //geometry_msgs::Quaternion q0 = tf::createQuaternionMsgFromRollPitchYaw(roll0, pitch0, yaw0); 217 | Eigen::Quaterniond q_temp; 218 | q_temp.x()=q0.x; 219 | q_temp.y()=q0.y; 220 | q_temp.z()=q0.z; 221 | q_temp.w()=q0.w; 222 | Eigen::Quaterniond q1 = q_temp * extQRPY2; 223 | 224 | // float r_x = q1.x(); 225 | // float r_y = q1.y(); 226 | // float r_z = q1.z(); 227 | // float r_w = q1.w(); 228 | 229 | float r_x = q_temp.x(); 230 | float r_y = q_temp.y(); 231 | float r_z = q_temp.z(); 232 | float r_w = q_temp.w(); 233 | 234 | gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z)); 235 | 236 | // 0. initialize gps 237 | if (gpsInitialized == false) 238 | { 239 | resetOptimization(); 240 | 241 | // pop old IMU message 242 | while (!imuQueOpt.empty()) 243 | { 244 | if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t) 245 | { 246 | lastImuT_opt = ROS_TIME(&imuQueOpt.front()); 247 | imuQueOpt.pop_front(); 248 | } 249 | else 250 | break; 251 | } 252 | // initial pose 253 | prevPose_ = lidarPose.compose(lidar2Imu); 254 | gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise); 255 | graphFactors.add(priorPose); 256 | // initial velocity 257 | prevVel_ = gtsam::Vector3(0, 0, 0); 258 | gtsam::PriorFactor priorVel(V(0), prevVel_, priorVelNoise); 259 | graphFactors.add(priorVel); 260 | // initial bias 261 | prevBias_ = gtsam::imuBias::ConstantBias(); 262 | gtsam::PriorFactor priorBias(B(0), prevBias_, priorBiasNoise); 263 | graphFactors.add(priorBias); 264 | // add values 265 | graphValues.insert(X(0), prevPose_); 266 | graphValues.insert(V(0), prevVel_); 267 | graphValues.insert(B(0), prevBias_); 268 | // optimize once 269 | optimizer.update(graphFactors, graphValues); 270 | graphFactors.resize(0); 271 | graphValues.clear(); 272 | 273 | imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_); 274 | imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); 275 | 276 | key = 1; 277 | 278 | gpsInitialized = true; 279 | //ROS_INFO("initialzed!"); 280 | return; 281 | } 282 | 283 | // reset graph for speed 284 | if (key == 100) 285 | { 286 | // get updated noise before reset 287 | gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1))); 288 | gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1))); 289 | gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1))); 290 | // reset graph 291 | resetOptimization(); 292 | // add pose 293 | gtsam::PriorFactor priorPose(X(0), prevPose_, updatedPoseNoise); 294 | graphFactors.add(priorPose); 295 | // add velocity 296 | gtsam::PriorFactor priorVel(V(0), prevVel_, updatedVelNoise); 297 | graphFactors.add(priorVel); 298 | // add bias 299 | gtsam::PriorFactor priorBias(B(0), prevBias_, updatedBiasNoise); 300 | graphFactors.add(priorBias); 301 | // add values 302 | graphValues.insert(X(0), prevPose_); 303 | graphValues.insert(V(0), prevVel_); 304 | graphValues.insert(B(0), prevBias_); 305 | // optimize once 306 | optimizer.update(graphFactors, graphValues); 307 | graphFactors.resize(0); 308 | graphValues.clear(); 309 | 310 | key = 1; 311 | } 312 | 313 | // 1. integrate imu data and optimize 314 | while (!imuQueOpt.empty()) 315 | { 316 | // pop and integrate imu data that is between two optimizations 317 | sensor_msgs::Imu *thisImu = &imuQueOpt.front(); 318 | double imuTime = ROS_TIME(thisImu); 319 | if (imuTime < currentCorrectionTime - delta_t) 320 | { 321 | double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt); 322 | // ROS_INFO("dt = %lf",dt); 323 | if (dt == 0.0) 324 | dt = 1.0/500.0; 325 | imuIntegratorOpt_->integrateMeasurement( 326 | gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z), 327 | gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt); 328 | 329 | lastImuT_opt = imuTime; 330 | imuQueOpt.pop_front(); 331 | } 332 | else 333 | break; 334 | } 335 | // add imu factor to graph 336 | const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast(*imuIntegratorOpt_); 337 | gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu); 338 | graphFactors.add(imu_factor); 339 | // add imu bias between factor 340 | graphFactors.add(gtsam::BetweenFactor(B(key - 1), B(key), gtsam::imuBias::ConstantBias(), 341 | gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias))); 342 | // add gps factor 343 | /*gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu); 344 | gtsam::PriorFactor pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise); 345 | graphFactors.add(pose_factor);*/ 346 | gtsam::Vector Vector3(3); 347 | Vector3 << 1.0f, 1.0f, 1.0f; 348 | noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3); 349 | gtsam::GPSFactor gps_factor(X(key), gtsam::Point3(p_x, p_y, p_z), gps_noise); 350 | graphFactors.add(gps_factor); 351 | // insert predicted values 352 | gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_); 353 | graphValues.insert(X(key), propState_.pose()); 354 | graphValues.insert(V(key), propState_.v()); 355 | graphValues.insert(B(key), prevBias_); 356 | // optimize 357 | optimizer.update(graphFactors, graphValues); 358 | optimizer.update(); 359 | graphFactors.resize(0); 360 | graphValues.clear(); 361 | // Overwrite the beginning of the preintegration for the next step. 362 | gtsam::Values result = optimizer.calculateEstimate(); 363 | prevPose_ = result.at(X(key)); 364 | prevVel_ = result.at(V(key)); 365 | prevState_ = gtsam::NavState(prevPose_, prevVel_); 366 | prevBias_ = result.at(B(key)); 367 | // Reset the optimization preintegration object. 368 | imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); 369 | 370 | // 2. after optimization, re-propagate imu odometry preintegration 371 | prevStateOdom = prevState_; 372 | prevBiasOdom = prevBias_; 373 | // first pop imu message older than current correction data 374 | double lastImuQT = -1; 375 | while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t) 376 | { 377 | lastImuQT = ROS_TIME(&imuQueImu.front()); 378 | imuQueImu.pop_front(); 379 | } 380 | // repropogate 381 | if (!imuQueImu.empty()) 382 | { 383 | // reset bias use the newly optimized bias 384 | imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom); 385 | // integrate imu message from the beginning of this optimization 386 | for (int i = 0; i < (int)imuQueImu.size(); ++i) 387 | { 388 | sensor_msgs::Imu *thisImu = &imuQueImu[i]; 389 | double imuTime = ROS_TIME(thisImu); 390 | double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT); 391 | if (dt == 0.0) 392 | dt = 1.0/500.0; 393 | imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z), 394 | gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt); 395 | lastImuQT = imuTime; 396 | } 397 | } 398 | 399 | ++key; 400 | doneFirstOpt = true; 401 | 402 | } 403 | 404 | bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur) 405 | { 406 | Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z()); 407 | if (vel.norm() > 30) 408 | { 409 | ROS_WARN("Large velocity, reset IMU-preintegration!"); 410 | return true; 411 | } 412 | 413 | Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z()); 414 | Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z()); 415 | if (ba.norm() > 1.0 || bg.norm() > 1.0) 416 | { 417 | ROS_WARN("Large bias, reset IMU-preintegration!"); 418 | return true; 419 | } 420 | 421 | return false; 422 | } 423 | 424 | void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw) 425 | { 426 | ROS_INFO("into imuhandler"); 427 | sensor_msgs::Imu thisImu = *imu_raw; 428 | //Convert IMU 429 | thisImu.linear_acceleration.x = imu_raw->linear_acceleration.y; 430 | thisImu.linear_acceleration.y = -imu_raw->linear_acceleration.x; 431 | thisImu.angular_velocity.x = imu_raw->angular_velocity.y; 432 | thisImu.angular_velocity.y = -imu_raw->angular_velocity.x; 433 | 434 | imuQueOpt.push_back(thisImu); 435 | imuQueImu.push_back(thisImu); 436 | 437 | if (doneFirstOpt == false) 438 | return; 439 | 440 | double imuTime = ROS_TIME(&thisImu); 441 | double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu); 442 | lastImuT_imu = imuTime; 443 | if (dt == 0.0) 444 | dt = 1.0/500.0; 445 | // integrate this single imu message 446 | imuIntegratorImu_->integrateMeasurement( 447 | gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, 448 | thisImu.linear_acceleration.z), 449 | gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), 450 | dt); 451 | 452 | // predict odometry 453 | gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 454 | //quat convert 455 | Eigen::Vector3d extV(-PI / 2, 0, 0); 456 | //Eigen::Vector3d extV(0, 0, 0); 457 | Eigen::Matrix3d extRPY1, extRPY2; 458 | Eigen::Quaterniond extQRPY1, extQRPY2; 459 | extRPY1 << 1, 0, 0, 460 | 0, -1, 0, 461 | 0, 0, 1; 462 | extQRPY1 = Eigen::Quaterniond(extRPY1); 463 | extRPY2 = Eigen::AngleAxisd(extV[0], Eigen::Vector3d::UnitZ()) 464 | * Eigen::AngleAxisd(extV[1], Eigen::Vector3d::UnitY()) 465 | * Eigen::AngleAxisd(extV[2], Eigen::Vector3d::UnitX()); 466 | //extQRPY2 = Eigen::Quaterniond(extRPY2); 467 | extQRPY2 = extRPY2; 468 | // publish odometry 469 | nav_msgs::Odometry odometry; 470 | odometry.header.stamp = thisImu.header.stamp; 471 | odometry.header.frame_id = "odometryFrame"; 472 | odometry.child_frame_id = "odom_imu"; 473 | 474 | // transform imu pose to ldiar 475 | gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position()); 476 | gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar); 477 | 478 | Eigen::Quaterniond q_final = extQRPY2 * lidarPose.rotation().toQuaternion(); 479 | odometry.pose.pose.position.x = lidarPose.translation().x(); 480 | odometry.pose.pose.position.y = lidarPose.translation().y(); 481 | odometry.pose.pose.position.z = lidarPose.translation().z(); 482 | 483 | odometry.pose.pose.orientation.x = q_final.x(); 484 | odometry.pose.pose.orientation.y = q_final.y(); 485 | odometry.pose.pose.orientation.z = q_final.z(); 486 | odometry.pose.pose.orientation.w = q_final.w(); 487 | 488 | odometry.twist.twist.linear.x = currentState.velocity().x(); 489 | odometry.twist.twist.linear.y = currentState.velocity().y(); 490 | odometry.twist.twist.linear.z = currentState.velocity().z(); 491 | odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x(); 492 | odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y(); 493 | odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z(); 494 | pubImuOdometry.publish(odometry); 495 | 496 | } 497 | 498 | }; 499 | 500 | int main(int argc, char **argv) { 501 | 502 | ros::init(argc, argv, "od_sins_node"); 503 | 504 | IMUPreintegration ImuP; 505 | 506 | ros::spin(); 507 | 508 | return 0; 509 | } 510 | 511 | -------------------------------------------------------------------------------- /include/PSINS.h: -------------------------------------------------------------------------------- 1 | #ifndef _PSINS_H 2 | #define _PSINS_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | 68 | #include 69 | #include 70 | 71 | #include 72 | #include 73 | //#include 74 | #include 75 | #include 76 | #include 77 | #include 78 | #include 79 | #include 80 | #include 81 | 82 | 83 | /************** compiling control !!! ***************/ 84 | #define PSINS_MATRIX_MAX_DIM 43 85 | //#define MAT_COUNT_STATISTIC 86 | #define PSINS_IO_FILE 87 | #define PSINS_RMEMORY 88 | //#define PSINS_AHRS_MEMS 89 | //#define PSINS_LOW_GRADE_MEMS 90 | //#define CSINSGPSOD_INIT_BY_USER 91 | //#define PSINS_psinsassert 92 | //#define PSINS_UART_PUSH_POP 93 | //#define PSINS_VC_AFX_HEADER 94 | 95 | // type re-define 96 | #ifndef BOOL 97 | typedef int BOOL; 98 | #endif 99 | 100 | #ifndef BYTE 101 | typedef unsigned char BYTE; 102 | #endif 103 | 104 | // constant define 105 | #ifndef TRUE 106 | #define TRUE 1 107 | #define FALSE 0 108 | #endif 109 | 110 | #ifndef NULL 111 | #define NULL ((void *)0) 112 | #endif 113 | 114 | #ifndef PI 115 | #define PI 3.14159265358979 116 | #endif 117 | #define PI_2 (PI/2.0) 118 | #define PI_4 (PI/4.0) 119 | #define _2PI (2.0*PI) 120 | 121 | #ifndef DEG 122 | #define DEG (PI/180.0) 123 | #endif 124 | #define DPS (DEG/1.0) // deg/s 125 | #define SEC (DEG/3600.0) // arcsec 126 | #define DPH (DEG/3600.0) // deg/h 127 | #define G0 9.7803267714 128 | #define UG (G0/1.0e6) // ug 129 | #define RE 6378137.0 130 | #define PPM 1.0e-6 131 | 132 | #ifndef EPS 133 | #define EPS 2.220446049e-16F 134 | #endif 135 | #ifndef INF 136 | #define INF 3.402823466e+30F 137 | #endif 138 | 139 | #ifdef PSINS_psinsassert 140 | BOOL psinspsinsassert(BOOL b); 141 | #else 142 | #define psinsassert(b) {}; 143 | #endif 144 | 145 | #ifndef max 146 | #define max(x,y) ( (x)>=(y)?(x):(y) ) 147 | #endif 148 | #ifndef min 149 | #define min(x,y) ( (x)<=(y)?(x):(y) ) 150 | #endif 151 | 152 | #define CC180toC360(yaw) ( (yaw)>0.0 ? (_2PI-(yaw)) : -(yaw) ) // counter-clockwise +-180deg -> clockwise 0~360deg for yaw 153 | #define C360toCC180(yaw) ( (yaw)>=PI ? (_2PI-(yaw)) : -(yaw) ) // clockwise 0~360deg -> counter-clockwise +-180deg for yaw 154 | 155 | //#define disp(k, nSec, frq) if(k%(nSec*frq)==0) printf("%d\n", k/frq) 156 | #define console_disp(i, FRQ, n) if((i)%((n)*(FRQ))==0) printf("%d\n", (i)/(FRQ)) 157 | 158 | // class define 159 | class CGLV; 160 | class CVect3; class CMat3; class CQuat; class CVect; class CMat; 161 | class CEarth; class CIMU; class CSINS; class CAVPInterp; class CAligni0; 162 | class CKalman; class CSINSKF; class CSINSTDKF; class CSINSGPSOD; class CAlignkf; 163 | class CRAvar; class CVAR; class CVARn; class CIIR; class CIIRV3; 164 | class CRMemory; class CFileRdWt; class CUartPP; 165 | 166 | // function define 167 | double r2dm(double r); 168 | double dm2r(double dm); 169 | //BOOL IsZero(double f, double eps = EPS); 170 | int sign(double val, double eps = EPS); 171 | double range(double val, double minVal, double maxVal); 172 | double atan2Ex(double y, double x); 173 | double diffYaw(double yaw, double yaw0); 174 | double MKQt(double sR, double tau); 175 | double randn(double mu, double sigma); 176 | #define pow2(x) ((x)*(x)) 177 | #define asinEx(x) asin(range(x, -1.0, 1.0)) 178 | #define acosEx(x) acos(range(x, -1.0, 1.0)) 179 | CVect3 q2att(const CQuat &qnb); 180 | CMat3 diag(const CVect3 &v); 181 | void IMURFU(CVect3 *pwm, int nSamples, const char *str); 182 | void IMURFU(CVect3 *pwm, CVect3 *pvm, int nSamples, const char *str); 183 | 184 | // Matrix Max Dimension define 185 | #define MMD PSINS_MATRIX_MAX_DIM 186 | #define MMD2 (MMD*MMD) 187 | 188 | // global variables and functions 189 | extern const CVect3 I31, O31, Ipos; 190 | extern const CQuat qI; 191 | extern const CMat3 I33, O33; 192 | extern const CVect On1, O1n; 193 | extern const CGLV glv; 194 | extern int psinslasterror; 195 | 196 | #define LLH(latitude,longitude,height) CVect3((latitude)*PI/180,(longitude)*PI/180,height) 197 | #define PRY(pitch,roll,yaw) CVect3((pitch)*PI/180,(roll)*PI/180,(yaw)*PI/180) 198 | 199 | class CGLV 200 | { 201 | public: 202 | double Re, f, g0, wie; // the Earth's parameters 203 | double e, e2, ep, ep2, Rp; 204 | double mg, ug, deg, min, sec, hur, ppm, ppmpsh; // commonly used units 205 | double dps, dph, dpsh, dphpsh, ugpsh, ugpsHz, ugpg2, mpsh, mpspsh, secpsh; 206 | 207 | CGLV(double Re = 6378137.0, double f = (1.0 / 298.257), double wie0 = 7.2921151467e-5, double g0 = 9.7803267714); 208 | }; 209 | 210 | class CVect3 211 | { 212 | public: 213 | double i, j, k; 214 | 215 | CVect3(void); 216 | CVect3(double xyz); 217 | CVect3(double xx, double yy, double zz); 218 | CVect3(const double *pdata); 219 | CVect3(const float *pdata); 220 | 221 | CVect3& operator=(double f); // every element equal to a same double 222 | CVect3& operator=(const double *pf); // vector equal to a array 223 | friend BOOL IsZero(const CVect3 &v, double eps); // psinsassert if all elements are zeros 224 | friend BOOL IsZeroXY(const CVect3 &v, double eps); // psinsassert if x&&y-elements are zeros 225 | friend BOOL IsNaN(const CVect3 &v); // psinsassert if any element is NaN 226 | CVect3 operator+(const CVect3 &v) const; // vector addition 227 | CVect3 operator-(const CVect3 &v) const; // vector subtraction 228 | CVect3 operator*(const CVect3 &v) const; // vector cross multiplication 229 | CVect3 operator*(const CMat3 &m) const; // row-vector multiply matrix 230 | CVect3 operator*(double f) const; // vector multiply scale 231 | CVect3 operator/(double f) const; // vector divide scale 232 | CVect3 operator/(const CVect3 &v) const; // vector divide vect3 element by element 233 | CVect3& operator+=(const CVect3 &v); // vector addition 234 | CVect3& operator-=(const CVect3 &v); // vector subtraction 235 | CVect3& operator*=(double f); // vector multiply scale 236 | CVect3& operator/=(double f); // vector divide scale 237 | CVect3& operator/=(const CVect3 &v); // vector divide vect3 element by element 238 | friend CVect3 operator*(double f, const CVect3 &v); // scale multiply vector 239 | friend CVect3 operator-(const CVect3 &v); // minus 240 | friend CMat3 vxv(const CVect3 &v1, const CVect3 &v2); // column-vector multiply row-vector, v1*v2' 241 | friend CVect3 abs(const CVect3 &v); // abs 242 | friend double norm(const CVect3 &v); // vector norm 243 | friend double normInf(const CVect3 &v); // vector inf-norm 244 | friend double normXY(const CVect3 &v); // vector norm of X & Y components 245 | friend CVect3 sqrt(const CVect3 &v); // sqrt 246 | friend CVect3 pow(const CVect3 &v, int k); // power 247 | friend double dot(const CVect3 &v1, const CVect3 &v2); // vector dot multiplication 248 | friend CMat3 a2mat(const CVect3 &att); // Euler angles to DCM 249 | friend CVect3 m2att(const CMat3 &Cnb); // DCM to Euler angles 250 | friend CQuat a2qua(double pitch, double roll, double yaw); // Euler angles to quaternion 251 | friend CQuat a2qua(const CVect3 &att); // Euler angles to quaternion 252 | friend CVect3 q2att(const CQuat &qnb); // quaternion to Euler angles 253 | friend CQuat rv2q(const CVect3 &rv); // rotation vector to quaternion 254 | friend CVect3 q2rv(const CQuat &q); // quaternion to rotation vector 255 | friend CMat3 askew(const CVect3 &v); // askew matrix; 256 | friend double sinAng(const CVect3 &v1, const CVect3 &v2); // |sin(angle(v1,v2))| 257 | friend CMat3 pos2Cen(const CVect3 &pos); // to geographical position matrix 258 | friend CVect3 pp2vn(const CVect3 &pos1, const CVect3 &pos0, double ts, CEarth *pEth); // position difference to velocity 259 | friend CVect3 MKQt(const CVect3 &sR, const CVect3 &tau);// first order Markov white-noise variance calculation 260 | friend CMat3 dv2att(CVect3 &va1, const CVect3 &va2, CVect3 &vb1, const CVect3 &vb2); // attitude determination using double-vector 261 | friend CVect3 Alignsb(CVect3 wmm, CVect3 vmm, double latitude); // align in static-base 262 | friend double MagYaw(const CVect3 &mag, const CVect3 &att, double declination); 263 | friend CVect3 xyz2blh(const CVect3 &xyz); // ECEF X/Y/Z to latitude/longitude/height 264 | friend CVect3 blh2xyz(const CVect3 &blh); // latitude/longitude/height to ECEF X/Y/Z 265 | friend CVect3 Vxyz2enu(const CVect3 &Vxyz, const CVect3 &pos); // ECEF Vx/Vy/Vz to Ve/Vn/Vu 266 | friend std::ofstream & operator<<(std::ofstream &out, CVect3 &A); 267 | }; 268 | 269 | class CQuat 270 | { 271 | public: 272 | double q0, q1, q2, q3; 273 | 274 | CQuat(void); 275 | CQuat(double qq0, double qq1 = 0.0, double qq2 = 0.0, double qq3 = 0.0); 276 | CQuat(const double *pdata); 277 | 278 | CQuat operator+(const CVect3 &phi) const; // true quaternion add misalign angles 279 | CQuat operator-(const CVect3 &phi) const; // calculated quaternion delete misalign angles 280 | CVect3 operator-(CQuat &quat) const; // get misalign angles from calculated quaternion & true quaternion 281 | CQuat operator*(const CQuat &q) const; // quaternion multiplication 282 | CVect3 operator*(const CVect3 &v) const; // quaternion multiply vector 283 | CQuat& operator*=(const CQuat &q); // quaternion multiplication 284 | CQuat& operator-=(const CVect3 &phi); // calculated quaternion delete misalign angles 285 | void SetYaw(double yaw = 0.0); // set Euler angles to designated yaw 286 | friend void normlize(CQuat *q); // quaternion norm 287 | friend CQuat operator~(const CQuat &q); // quaternion conjugate 288 | friend CVect3 qq2phi(const CQuat &qcalcu, const CQuat &qreal); 289 | friend CQuat UpDown(const CQuat &q); // Up-Down the quaternion represented attitide 290 | }; 291 | 292 | class CMat3 293 | { 294 | public: 295 | double e00, e01, e02, e10, e11, e12, e20, e21, e22; 296 | 297 | CMat3(void); 298 | CMat3(double xx, double xy, double xz, 299 | double yx, double yy, double yz, 300 | double zx, double zy, double zz); 301 | CMat3(const CVect3 &v0, const CVect3 &v1, const CVect3 &v2); // M = [v0; v1; v2] 302 | 303 | CMat3 operator+(const CMat3 &m) const; // matrix addition 304 | CMat3 operator-(const CMat3 &m) const; // matrix subtraction 305 | CMat3 operator*(const CMat3 &m) const; // matrix multiplication 306 | CMat3 operator*(double f) const; // matrix multiply scale 307 | CVect3 operator*(const CVect3 &v) const; // matrix multiply vector 308 | CMat3& operator+=(const CMat3 &m); // matrix += 309 | friend CMat3 operator-(const CMat3 &m); // minus 310 | friend CMat3 operator~(const CMat3 &m); // matrix transposition 311 | friend CMat3 operator*(double f, const CMat3 &m); // scale multiply matrix 312 | friend CMat3 pow(const CMat3 &m, int k); 313 | friend double trace(const CMat3 &m); // matrix trace 314 | friend double det(const CMat3 &m); // matrix determinat 315 | friend CMat3 adj(const CMat3 &m); // 3x3 adjoint matrix 316 | friend CMat3 inv(const CMat3 &m); // 3x3 matrix inverse 317 | friend CVect3 diag(const CMat3 &m); // the diagonal of a matrix 318 | friend CMat3 diag(const CVect3 &v); // diagonal matrix 319 | friend CQuat m2qua(const CMat3 &Cnb); // DCM to quaternion 320 | friend CMat3 q2mat(const CQuat &qnb); // attitude quaternion to DCM 321 | friend CMat3 foam(const CMat3 &B, int iter); // Fast Optimal Attitude Matrix(FOAM) 322 | friend std::ofstream & operator<<(std::ofstream &out, CMat3 &A); 323 | }; 324 | 325 | class CVect 326 | { 327 | public: 328 | int row, clm, rc; 329 | double dd[MMD]; 330 | 331 | CVect(void); 332 | CVect(int row0, int clm0 = 1); 333 | CVect(int row0, double f); 334 | CVect(int row0, double f, double f1, ...); 335 | CVect(int row0, const double *pf); 336 | CVect(const CVect3 &v); 337 | CVect(const CVect3 &v1, const CVect3 v2); 338 | 339 | void Set(double f, ...); 340 | void Set2(double f, ...); 341 | CVect operator+(const CVect &v) const; // vector addition 342 | CVect operator-(const CVect &v) const; // vector subtraction 343 | CVect operator*(double f) const; // vector multiply scale 344 | CVect& operator=(double f); // every element equal to a same double 345 | CVect& operator=(const double *pf); // vector equal to a array 346 | CVect& operator+=(const CVect &v); // vector addition 347 | CVect& operator-=(const CVect &v); // vector subtraction 348 | CVect& operator*=(double f); // vector multiply scale 349 | CVect operator*(const CMat &m) const; // row-vector multiply matrix 350 | CMat operator*(const CVect &v) const; // 1xn vector multiply nx1 vector, or nx1 vector multiply 1xn vector 351 | double& operator()(int r); // vector element 352 | friend CVect operator~(const CVect &v); // vector transposition 353 | friend CVect abs(const CVect &v); // vector abs 354 | friend double norm(const CVect &v); // vector norm 355 | friend double normInf(const CVect &v); // inf-norm 356 | friend CVect pow(const CVect &v, int k); // power 357 | friend std::ofstream & operator<<(std::ofstream &out, CVect &A); 358 | }; 359 | 360 | class CMat 361 | { 362 | public: 363 | int row, clm, rc; 364 | double dd[MMD2]; 365 | 366 | CMat(void); 367 | CMat(int row0, int clm0); 368 | CMat(int row0, int clm0, double f); 369 | CMat(int row0, int clm0, const double *pf); 370 | 371 | void Clear(void); 372 | void SetDiag(double f, ...); 373 | void SetDiag2(double f, ...); 374 | CMat operator+(const CMat &m) const; // matrix addition 375 | CMat operator-(const CMat &m) const; // matrix subtraction 376 | CMat operator*(double f) const; // matrix multiply scale 377 | CVect operator*(const CVect &v) const; // matrix multiply vector 378 | CMat operator*(const CMat &m) const; // matrix multiplication 379 | CMat& operator+=(const CMat &m0); // matrix addition 380 | CMat& operator+=(const CVect &v); // matrix + diag(vector) 381 | CMat& operator-=(const CMat &m0); // matrix subtraction 382 | CMat& operator*=(double f); // matrix multiply scale 383 | CMat& operator++(); // 1.0 + diagonal 384 | double& operator()(int r, int c = -1); // get element m(r,c) 385 | void ZeroRow(int i); // set i-row to 0 386 | void ZeroClm(int j); // set j-column to 0 387 | void SetRow(int i, double f, ...); // set i-row from n-double 388 | void SetRow(int i, const CVect &v); // set i-row from vector 389 | void SetClm(int j, double f, ...); // set j-column from n-double 390 | void SetClm(int j, const CVect &v); // set j-column from vector 391 | CVect GetRow(int i) const; // get i-row from matrix 392 | CVect GetClm(int j) const; // get j-column from matrix 393 | void SetRowVect3(int i, int j, const CVect3 &v); // set i-row&j...(j+2)-column from CVect3 394 | void SetClmVect3(int i, int j, const CVect3 &v); // set i...(i+2)-row&j-column from CVect3 395 | void SetDiagVect3(int i, int j, const CVect3 &v); // m(i,j)=v.i, m(i+1,j+1)=v.j, m(i+2,j+2)=v.k; 396 | CVect3 GetDiagVect3(int i, int j = -1); // return CVect3(m(i,j), m(i+1,j+1), m(i+2,j+2)) 397 | void SetAskew(int i, int j, const CVect3 &v); // set i...(i+2)-row&j...(j+2)-comumn from askew CVect3 398 | void SetMat3(int i, int j, const CMat3 &m); // set i...(i+2)-row&j...(j+2)-comumn from CMat3 399 | CMat3 GetMat3(int i, int j = -1) const; // get CMat3 from i...(i+2)-row&j...(j+2)-comumn 400 | void SubAddMat3(int i, int j, const CMat3 &m); // add i...(i+2)-row&j...(j+2)-comumn with CMat3 m 401 | friend CMat operator~(const CMat &m); // matrix transposition 402 | friend void symmetry(CMat &m); // matrix symmetrization 403 | friend double normInf(CMat &m); // inf-norm 404 | friend CVect diag(const CMat &m); // diagonal of a matrix 405 | friend CMat diag(const CVect &v); // diagonal matrix 406 | friend void RowMul(CMat &m, const CMat &m0, const CMat &m1, int r); // m(r,:)=m0(r,:)*m1 407 | friend void RowMulT(CMat &m, const CMat &m0, const CMat &m1, int r); // m(r,:)=m0(r,:)*m1' 408 | friend void DVMDVafa(const CVect &V, CMat &M, double afa); // M = diag(V)*M*diag(V)*afa 409 | friend std::ofstream & operator<<(std::ofstream &out, CMat &A); 410 | #ifdef MAT_COUNT_STATISTIC 411 | static int iCount, iMax; 412 | ~CMat(void); 413 | #endif 414 | }; 415 | 416 | class CRAvar 417 | { 418 | public: 419 | #define RAMAX MMD 420 | int nR0, Rmaxcount[RAMAX], Rmaxflag[RAMAX]; 421 | double ts, R0[RAMAX], Rmax[RAMAX], Rmin[RAMAX], tau[RAMAX], r0[RAMAX]; 422 | 423 | CRAvar(void); 424 | CRAvar(int nR0, int maxCount0 = 2); 425 | void set(double r0, double tau, double rmax = 0.0, double rmin = 0.0, int i = 0); 426 | void set(const CVect3 &r0, const CVect3 &tau, const CVect3 &rmax = O31, const CVect3 &rmin = O31); 427 | void set(const CVect &r0, const CVect &tau, const CVect &rmax = On1, const CVect &rmin = On1); 428 | void Update(double r, double ts, int i = 0); 429 | void Update(const CVect3 &r, double ts); 430 | void Update(const CVect &r, double ts); 431 | double operator()(int k); // get element sqrt(R0(k)) 432 | }; 433 | 434 | class CVAR 435 | { 436 | public: 437 | #define VARMAX 50 438 | int ipush, imax; 439 | float array[VARMAX], mean, var; 440 | 441 | CVAR(int imax0 = 10, float data0 = 0.0); 442 | float Update(float data, BOOL isvar = TRUE); 443 | }; 444 | 445 | class CVARn { 446 | public: 447 | int row, clm, idxpush, rowcnt; 448 | double **pData, *pd, *Sx, *Sx2, *mx, *stdx; // sum(x), sum(x^2), mean(x), std(x) 449 | double stdsf; // std scalefactor 450 | CVARn(void); 451 | CVARn(int row0, int clm0); 452 | ~CVARn(void); 453 | void Reset(void); 454 | BOOL Update(const double *pd); 455 | BOOL Update(double f, ...); 456 | }; 457 | 458 | class CEarth 459 | { 460 | public: 461 | double a, b; 462 | double f, e, e2; 463 | double wie; 464 | 465 | double sl, sl2, sl4, cl, tl, RMh, RNh, clRNh, f_RMh, f_RNh, f_clRNh; 466 | CVect3 pos, vn, wnie, wnen, wnin, gn, gcc, *pgn; 467 | 468 | CEarth(double a0 = glv.Re, double f0 = glv.f, double g0 = glv.g0); 469 | void Update(const CVect3 &pos, const CVect3 &vn = O31); 470 | CVect3 vn2dpos(const CVect3 &vn, double ts = 1.0) const; 471 | }; 472 | 473 | class CEGM // Earth Gravitational Model 474 | { 475 | public: 476 | CEGM(void); 477 | int Init(const char *fileModel); 478 | int Init(double GM0, double Re0, double ff0, double wie0, double *pC0, double *pS0, int N0); 479 | ~CEGM(void); 480 | void Update(double *blh, double *gn, int degree = -1); 481 | }; 482 | 483 | class CIMU 484 | { 485 | public: 486 | int nSamples; 487 | bool preFirst, onePlusPre; 488 | CVect3 phim, dvbm, wm_1, vm_1; 489 | 490 | CIMU(void); 491 | void Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples); 492 | friend void IMURFU(CVect3 *pwm, int nSamples, const char *str); 493 | friend void IMURFU(CVect3 *pwm, CVect3 *pvm, int nSamples, const char *str); 494 | }; 495 | 496 | class CSINS // sizeof(CSINS)~=3k bytes 497 | { 498 | public: 499 | double ts, nts, tk, velMax, hgtMin, hgtMax; 500 | CEarth eth; 501 | CIMU imu; 502 | CQuat qnb; 503 | CMat3 Cnb, Cnb0, Cbn, Kg, Ka; 504 | CVect3 wib, fb, fn, an, web, wnb, att, vn, vb, pos, eb, db, Ka2, tauGyro, tauAcc, _betaGyro, _betaAcc; 505 | CMat3 Maa, Mav, Map, Mva, Mvv, Mvp, Mpv, Mpp; // for etm 506 | CVect3 lvr, vnL, posL; CMat3 CW, MpvCnb; // for lever arm 507 | CQuat qnbE; CVect3 attE, vnE, posE; // for extrapolation 508 | CRAvar Rwfa; 509 | BOOL isOpenloop; 510 | 511 | CSINS(const CVect3 &att0, const CVect3 &vn0 = O31, const CVect3 &pos0 = O31, double tk0 = 0.0); 512 | CSINS(const CQuat &qnb0 = qI, const CVect3 &vn0 = O31, const CVect3 &pos0 = O31, double tk0 = 0.0); 513 | void Init(const CQuat &qnb0 = qI, const CVect3 &vn0 = O31, const CVect3 &pos0 = O31, double tk0 = 0.0); // initialization using quat attitude, velocity & position 514 | void SetTauGA(const CVect3 &tauG, const CVect3 &tauA); 515 | void Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts); // SINS update using Gyro&Acc samples 516 | void Extrap(const CVect3 &wm = O31, const CVect3 &vm = O31, double ts = 0.0); // SINS fast extrapolation using 1 Gyro&Acc sample 517 | void Extrap(double extts); // SINS fast extrapolation using previous Gyro&Acc sample 518 | void lever(const CVect3 &dL = O31); // lever arm 519 | void etm(void); // SINS error transform matrix coefficients 520 | }; 521 | 522 | class CAVPInterp 523 | { 524 | #define AVPINUM 10 525 | public: 526 | double ts; 527 | int ipush; 528 | CVect3 atti[AVPINUM], vni[AVPINUM], posi[AVPINUM]; 529 | CVect3 att, vn, pos; 530 | CAVPInterp(double ts = 0.01); 531 | void Push(const CVect3 &attk, const CVect3 &vnk = O31, const CVect3 &posk = O31); 532 | int Interp(double tpast); // AVP interpolation, where -AVPINUM*ts<=tpast<=0 533 | }; 534 | 535 | class CIIR 536 | { 537 | public: 538 | #define IIRnMax 6 539 | int n; 540 | double b[IIRnMax], a[IIRnMax], x[IIRnMax], y[IIRnMax]; 541 | 542 | CIIR(void); 543 | CIIR(double *b0, double *a0, int n0); 544 | double Update(double x0); 545 | }; 546 | 547 | class CIIRV3 548 | { 549 | public: 550 | CIIR iir0, iir1, iir2; 551 | CVect3 y; 552 | 553 | CIIRV3(void); 554 | CIIRV3(double *b0, double *a0, int n0, 555 | double *b1 = (double*)NULL, double *a1 = (double*)NULL, int n1 = 0, 556 | double *b2 = (double*)NULL, double *a2 = (double*)NULL, int n2 = 0); 557 | CVect3 Update(CVect3 &x); 558 | }; 559 | 560 | class CAligni0 561 | { 562 | public: 563 | int velAid, t0, t1, t2; 564 | CVect3 vel0, wmm, vmm, vib0, vi0, Pib01, Pib02, Pi01, Pi02, tmpPib0, tmpPi0; 565 | CQuat qib0b; 566 | CEarth eth; 567 | CIMU imu; 568 | double tk; 569 | CQuat qnb0, qnb, qnbsb; 570 | 571 | CAligni0(const CVect3 &pos0 = O31, const CVect3 &vel0 = O31, int velAid = 0); 572 | CQuat Update(const CVect3 *wm, const CVect3 *vm, int nSamples, double ts, const CVect3 &vel = O31); 573 | }; 574 | 575 | class CKalman 576 | { 577 | public: 578 | double kftk, zfdafa; 579 | int nq, nr, measflag, measflaglog, measstop; 580 | CMat Ft, Pk, Hk, Fading; 581 | CVect Xk, Zk, Qt, Rt, rts, RtTau, measlost, Xmax, Pmax, Pmin, Pset, Zfd, Zfd0, 582 | Rmax, Rmin, Rbeta, Rb, // measurement noise R adaptive 583 | FBTau, FBMax, FBOne, FBXk, FBTotal; // feedback control 584 | int Rmaxcount[MMD], Rmaxcount0[MMD]; 585 | // CRAvar Ravar; 586 | 587 | CKalman(int nq0, int nr0); 588 | virtual void Init(void) = 0; // initialize Qk,Rk,P0... 589 | virtual void SetFt(int nnq = 15) = 0; // process matrix setting 590 | virtual void SetHk(int nnq = 15) = 0; // measurement matrix setting 591 | virtual void SetMeas(void) = 0; // set measurement 592 | virtual void Feedback(double fbts); // feed back 593 | void RtFading(int i, double fdts); // Rt growing if no measurment 594 | void TimeUpdate(double kfts, int fback = 1); // time update 595 | int MeasUpdate(double fading = 1.0); // measurement update 596 | int RAdaptive(int i, double r, double Pr); // Rt adaptive 597 | void RPkFading(int i); // multiple fading 598 | void SetMeasFlag(int flag); // measurement flag setting 599 | void XPConstrain(void); // Xk & Pk constrain: -Xmax>(double &d); 741 | CFileRdWt& operator>>(CVect3 &v); 742 | CFileRdWt& operator>>(CQuat &q); 743 | CFileRdWt& operator>>(CMat3 &m); 744 | CFileRdWt& operator>>(CVect &v); 745 | CFileRdWt& operator>>(CMat &m); 746 | friend char* time2fname(void); 747 | }; 748 | 749 | 750 | #endif // PSINS_IO_FILE 751 | 752 | #ifdef PSINS_RMEMORY 753 | 754 | #define MAX_RECORD_BYTES 512 755 | 756 | class CRMemory 757 | { 758 | public: 759 | BYTE *pMemStart0, *pMemStart, *pMemEnd; 760 | int pushLen, popLen, recordLen; 761 | long memLen, dataLen; 762 | BYTE *pMemPush, *pMemPop, pushBuf[MAX_RECORD_BYTES], popBuf[MAX_RECORD_BYTES]; 763 | 764 | CRMemory(long recordNum, int recordLen0); 765 | CRMemory(BYTE *pMem, long memLen0, int recordLen0 = 0); 766 | ~CRMemory(); 767 | BOOL push(const BYTE *p = (const BYTE*)NULL); 768 | BYTE pop(BYTE *p = (BYTE*)NULL); 769 | BYTE* get(int iframe); 770 | }; 771 | 772 | #endif // PSINS_RMEMORY 773 | 774 | #ifdef PSINS_UART_PUSH_POP 775 | 776 | class CUartPP 777 | { 778 | public: 779 | #define UARTFRMLEN (50*4) 780 | #define UARTBUFLEN (UARTFRMLEN*20) 781 | unsigned char head[2], popbuf[UARTFRMLEN], buf[UARTBUFLEN], chksum; 782 | int pushIdx, popIdx, frameLen, overflow, getframe; 783 | int csflag, cs0, cs1, css; // 0: no checksum, 1: unsigned char sum, 2: crc8; popbuf[css]==popbuf[cs0]+...+popbuf[cs1] 784 | //unsigned short chksum; 785 | 786 | CUartPP(int frameLen0, unsigned short head0 = 0x55aa); // NOTE: char {0xaa 0x55} 787 | BOOL checksum(const unsigned char *pc); 788 | int nextIdx(int idx); 789 | int push(const unsigned char *buf0, int len); 790 | int pop(unsigned char *buf0 = (unsigned char*)NULL); 791 | }; 792 | 793 | #endif // PSINS_UART_PUSH_POP 794 | 795 | unsigned short swap16(unsigned short ui16); 796 | unsigned char* swap24(unsigned char* puc3, unsigned char* pres = NULL); 797 | unsigned int swap32(unsigned int ui32); 798 | unsigned long swap64(unsigned long ui64); 799 | unsigned char* int24(unsigned char *pchar3, int int32); 800 | int diffint24(const unsigned char *pc1, const unsigned char *pc0); 801 | 802 | #ifdef PSINS_VC_AFX_HEADER 803 | 804 | #include "afx.h" 805 | class CVCFileFind { 806 | public: 807 | BOOL lastfile; 808 | char fname[256]; 809 | CFileFind finder; 810 | CVCFileFind(char *path, char *filetype); 811 | char* FindNextFile(void); 812 | }; 813 | 814 | #endif // PSINS_VC_AFX_HEADER 815 | 816 | typedef struct { 817 | unsigned short head, chksum; 818 | float t, gx, gy, gz, ax, ay, az, magx, magy, magz, bar, pch, rll, yaw, ve, vn, vu, 819 | lon0, lon1, lat0, lat1, hgt, gpsve, gpsvn, gpsvu, gpslon0, gpslon1, gpslat0, gpslat1, gpshgt, 820 | gpsstat, gpsdly, tmp, rsv; 821 | } PSINSBoard; 822 | 823 | #endif 824 | void fusion(double *x1, double *p1, const double *x2, const double *p2, int n, double *xf, double *pf); 825 | 826 | -------------------------------------------------------------------------------- /.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 103 | 104 | 106 | 107 | 108 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 129 | 130 | 131 | 132 | 133 | 135 | 136 | 137 | 138 | 140 | 141 | 142 | 143 | 145 | 146 | 147 | 148 | 150 | 151 | 152 | 153 | 155 | 156 | 157 | 158 | 160 | 161 | 162 | 163 | 165 | 166 | 167 | 168 | 170 | 171 | 172 | 173 | 175 | 176 | 177 | 178 | 180 | 181 | 182 | 183 | 185 | 186 | 187 | 188 | 190 | 191 | 192 | 193 | 195 | 196 | 197 | 198 | 200 | 201 | 202 | 203 | 205 | 206 | 207 | 208 | 210 | 211 | 212 | 213 | 215 | 216 | 217 | 218 | 220 | 221 | 222 | 223 | 225 | 226 | 227 | 228 | 230 | 231 | 232 | 233 | 235 | 236 | 237 | 238 | 240 | 241 | 242 | 243 | 245 | 246 | 247 | 248 | 250 | 251 | 252 | 253 | 255 | 256 | 257 | 258 | 260 | 261 | 262 | 263 | 265 | 266 | 267 | 268 | 270 | 271 | 272 | 273 | 275 | 276 | 277 | 278 | 280 | 281 | 282 | 283 | 285 | 286 | 287 | 288 | 290 | 291 | 292 | 293 | 295 | 296 | 297 | 298 | 300 | 301 | 302 | 303 | 305 | 306 | 307 | 308 | 310 | 311 | 312 | 313 | 315 | 316 | 317 | 318 | 320 | 321 | 322 | 323 | 325 | 326 | 327 | 328 | 330 | 331 | 332 | 333 | 335 | 336 | 337 | 338 | 340 | 341 | 342 | 343 | 345 | 346 | 347 | 348 | 350 | 351 | 352 | 353 | 355 | 356 | 357 | 358 | 360 | 361 | 362 | 363 | 365 | 366 | 367 | 368 | 370 | 371 | 372 | 373 | 375 | 376 | 377 | 378 | 380 | 381 | 382 | 383 | 385 | 386 | 387 | 388 | 390 | 391 | 392 | 393 | 395 | 396 | 397 | 398 | 400 | 401 | 402 | 403 | 405 | 406 | 407 | 408 | 410 | 411 | 412 | 413 | 415 | 416 | 417 | 418 | 420 | 421 | 422 | 423 | 425 | 426 | 427 | 428 | 430 | 431 | 432 | 433 | 435 | 436 | 437 | 438 | 440 | 441 | 442 | 443 | 445 | 446 | 447 | 448 | 450 | 451 | 452 | 453 | 455 | 456 | 457 | 458 | 460 | 461 | 462 | 463 | 465 | 466 | 467 | 468 | 470 | 471 | 472 | 473 | 475 | 476 | 477 | 478 | 480 | 481 | 482 | 483 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 1619677153226 567 | 580 | 581 | 582 | 583 | 585 | 586 | 595 | 596 | -------------------------------------------------------------------------------- /src/PSINS.cpp: -------------------------------------------------------------------------------- 1 | #include "PSINS.h" 2 | 3 | const CVect3 I31(1.0), O31(0.0), Ipos(1.0 / RE, 1.0 / RE, 1.0); 4 | const CQuat qI(1.0, 0.0, 0.0, 0.0); 5 | const CMat3 I33(1, 0, 0, 0, 1, 0, 0, 0, 1), O33(0, 0, 0, 0, 0, 0, 0, 0, 0); 6 | const CVect On1(MMD, 0.0), O1n = ~On1; 7 | const CGLV glv; 8 | int psinslasterror = 0; 9 | 10 | /*************************** class CGLV *********************************/ 11 | CGLV::CGLV(double Re, double f, double wie0, double g0) 12 | { 13 | this->Re = Re; this->f = f; this->wie = wie0; this->g0 = g0; 14 | Rp = (1 - f)*Re; 15 | e = sqrt(2 * f - f * f); e2 = e * e; 16 | ep = sqrt(Re*Re - Rp * Rp) / Rp; ep2 = ep * ep; 17 | mg = g0 / 1000.0; 18 | ug = mg / 1000.0; 19 | deg = PI / 180.0; 20 | min = deg / 60.0; 21 | sec = min / 60.0; 22 | ppm = 1.0e-6; 23 | hur = 3600.0; 24 | dps = deg / 1.0; 25 | dph = deg / hur; 26 | dpsh = deg / sqrt(hur); 27 | dphpsh = dph / sqrt(hur); 28 | ugpsHz = ug / sqrt(1.0); 29 | ugpsh = ug / sqrt(hur); 30 | ugpg2 = ug / g0 / g0; 31 | mpsh = 1 / sqrt(hur); 32 | mpspsh = 1 / 1 / sqrt(hur); 33 | ppmpsh = ppm / sqrt(hur); 34 | secpsh = sec / sqrt(hur); 35 | } 36 | 37 | /*************************** class CVect3 *********************************/ 38 | CVect3::CVect3(void) 39 | { 40 | } 41 | 42 | CVect3::CVect3(double xyz) 43 | { 44 | i = j = k = xyz; 45 | } 46 | 47 | CVect3::CVect3(double xx, double yy, double zz) 48 | { 49 | i = xx, j = yy, k = zz; 50 | } 51 | 52 | CVect3::CVect3(const double *pdata) 53 | { 54 | i = *pdata++, j = *pdata++, k = *pdata; 55 | } 56 | 57 | CVect3::CVect3(const float *pdata) 58 | { 59 | i = *pdata++, j = *pdata++, k = *pdata; 60 | } 61 | 62 | BOOL IsZero(const CVect3 &v, double eps) 63 | { 64 | return (v.i-eps && v.j-eps && v.k-eps); 65 | } 66 | 67 | BOOL IsZeroXY(const CVect3 &v, double eps = EPS) 68 | { 69 | return (v.i-eps && v.j-eps); 70 | } 71 | 72 | BOOL IsNaN(const CVect3 &v) 73 | { 74 | return 0; //(_isnan(i) || _isnan(j) || _isnan(k)); 75 | } 76 | 77 | CVect3 CVect3::operator+(const CVect3 &v) const 78 | { 79 | return CVect3(this->i + v.i, this->j + v.j, this->k + v.k); 80 | } 81 | 82 | CVect3 CVect3::operator-(const CVect3 &v) const 83 | { 84 | return CVect3(this->i - v.i, this->j - v.j, this->k - v.k); 85 | } 86 | 87 | CVect3 CVect3::operator*(const CVect3 &v) const 88 | { 89 | return CVect3(this->j*v.k - this->k*v.j, this->k*v.i - this->i*v.k, this->i*v.j - this->j*v.i); 90 | } 91 | 92 | CVect3 CVect3::operator*(double f) const 93 | { 94 | return CVect3(i*f, j*f, k*f); 95 | } 96 | 97 | CVect3 CVect3::operator*(const CMat3 &m) const 98 | { 99 | return CVect3(i*m.e00 + j * m.e10 + k * m.e20, i*m.e01 + j * m.e11 + k * m.e21, i*m.e02 + j * m.e12 + k * m.e22); 100 | } 101 | 102 | CVect3 CVect3::operator/(double f) const 103 | { 104 | return CVect3(i / f, j / f, k / f); 105 | } 106 | 107 | CVect3 CVect3::operator/(const CVect3 &v) const 108 | { 109 | return CVect3(i / v.i, j / v.j, k / v.k); 110 | } 111 | 112 | CVect3& CVect3::operator=(double f) 113 | { 114 | i = j = k = f; 115 | return *this; 116 | } 117 | 118 | CVect3& CVect3::operator=(const double *pf) 119 | { 120 | i = *pf++, j = *pf++, k = *pf; 121 | return *this; 122 | } 123 | 124 | CVect3& CVect3::operator+=(const CVect3 &v) 125 | { 126 | i += v.i, j += v.j, k += v.k; 127 | return *this; 128 | } 129 | 130 | CVect3& CVect3::operator-=(const CVect3 &v) 131 | { 132 | i -= v.i, j -= v.j, k -= v.k; 133 | return *this; 134 | } 135 | 136 | CVect3& CVect3::operator*=(double f) 137 | { 138 | i *= f, j *= f, k *= f; 139 | return *this; 140 | } 141 | 142 | CVect3& CVect3::operator/=(double f) 143 | { 144 | i /= f, j /= f, k /= f; 145 | return *this; 146 | } 147 | 148 | CVect3& CVect3::operator/=(const CVect3 &v) 149 | { 150 | i /= v.i, j /= v.j, k /= v.k; 151 | return *this; 152 | } 153 | 154 | CVect3 operator*(double f, const CVect3 &v) 155 | { 156 | return CVect3(v.i*f, v.j*f, v.k*f); 157 | } 158 | 159 | CVect3 operator-(const CVect3 &v) 160 | { 161 | return CVect3(-v.i, -v.j, -v.k); 162 | } 163 | 164 | CMat3 vxv(const CVect3 &v1, const CVect3 &v2) 165 | { 166 | return CMat3(v1.i*v2.i, v1.i*v2.j, v1.i*v2.k, 167 | v1.j*v2.i, v1.j*v2.j, v1.j*v2.k, 168 | v1.k*v2.i, v1.k*v2.j, v1.k*v2.k); 169 | } 170 | 171 | CVect3 sqrt(const CVect3 &v) 172 | { 173 | return CVect3(sqrt(v.i), sqrt(v.j), sqrt(v.k)); 174 | } 175 | 176 | CVect3 pow(const CVect3 &v, int k = 2) 177 | { 178 | CVect3 pp = v; 179 | for (int i = 1; i < k; i++) 180 | { 181 | pp.i *= v.i, pp.j *= v.j, pp.k *= v.k; 182 | } 183 | return pp; 184 | } 185 | 186 | CVect3 abs(const CVect3 &v) 187 | { 188 | CVect3 res; 189 | res.i = v.i > 0.0 ? v.i : -v.i; 190 | res.j = v.j > 0.0 ? v.j : -v.j; 191 | res.k = v.k > 0.0 ? v.k : -v.k; 192 | return res; 193 | } 194 | 195 | double norm(const CVect3 &v) 196 | { 197 | return sqrt(v.i*v.i + v.j*v.j + v.k*v.k); 198 | } 199 | 200 | double normInf(const CVect3 &v) 201 | { 202 | double i = v.i > 0 ? v.i : -v.i, 203 | j = v.j > 0 ? v.j : -v.j, 204 | k = v.k > 0 ? v.k : -v.k; 205 | if (i > j) return i > k ? i : k; 206 | else return j > k ? j : k; 207 | } 208 | 209 | double normXY(const CVect3 &v) 210 | { 211 | return sqrt(v.i*v.i + v.j*v.j); 212 | } 213 | 214 | double dot(const CVect3 &v1, const CVect3 &v2) 215 | { 216 | return (v1.i*v2.i + v1.j*v2.j + v1.k*v2.k); 217 | } 218 | 219 | CQuat rv2q(const CVect3 &rv) 220 | { 221 | #define F1 ( 2 * 1) // define: Fk=2^k*k! 222 | #define F2 (F1*2 * 2) 223 | #define F3 (F2*2 * 3) 224 | #define F4 (F3*2 * 4) 225 | #define F5 (F4*2 * 5) 226 | double n2 = rv.i*rv.i + rv.j*rv.j + rv.k*rv.k, c, f; 227 | if (n2 < (PI / 180.0*PI / 180.0)) // 0.017^2 228 | { 229 | double n4 = n2 * n2; 230 | c = 1.0 - n2 * (1.0 / F2) + n4 * (1.0 / F4); 231 | f = 0.5 - n2 * (1.0 / F3) + n4 * (1.0 / F5); 232 | } 233 | else 234 | { 235 | double n_2 = sqrt(n2) / 2.0; 236 | c = cos(n_2); 237 | f = sin(n_2) / n_2 * 0.5; 238 | } 239 | return CQuat(c, f*rv.i, f*rv.j, f*rv.k); 240 | } 241 | 242 | CMat3 askew(const CVect3 &v) 243 | { 244 | return CMat3(0, -v.k, v.j, 245 | v.k, 0.0, -v.i, 246 | -v.j, v.i, 0); 247 | } 248 | 249 | CMat3 pos2Cen(const CVect3 &pos) 250 | { 251 | double si = sin(pos.i), ci = cos(pos.i), sj = sin(pos.j), cj = cos(pos.j); 252 | return CMat3(-sj, -si * cj, ci*cj, 253 | cj, -si * sj, ci*sj, 254 | 0, ci, si); //Cen 255 | } 256 | 257 | CVect3 pp2vn(const CVect3 &pos1, const CVect3 &pos0, double ts = 1.0, CEarth *pEth = (CEarth*)NULL) 258 | { 259 | double sl, cl, sl2, sq, sq2, RMh, RNh, clRNh; 260 | if (pEth) 261 | { 262 | RMh = pEth->RMh; clRNh = pEth->clRNh; 263 | } 264 | else 265 | { 266 | sl = sin(pos0.i); cl = cos(pos0.i); sl2 = sl * sl; 267 | sq = 1 - glv.e2*sl2; sq2 = sqrt(sq); 268 | RMh = glv.Re*(1 - glv.e2) / sq / sq2 + pos0.k; 269 | RNh = glv.Re / sq2 + pos0.k; clRNh = cl * RNh; 270 | } 271 | CVect3 vn = pos1 - pos0; 272 | return CVect3(vn.j*clRNh / ts, vn.i*RMh / ts, vn.k / ts); 273 | } 274 | 275 | double sinAng(const CVect3 &v1, const CVect3 &v2) 276 | { 277 | if (IsZero(v1, EPS) || IsZero(v2, EPS)) return 0.0; 278 | return norm(v1*v2) / (norm(v1)*norm(v2)); 279 | } 280 | 281 | double MagYaw(const CVect3 &mag, const CVect3 &att, double declination = 0) 282 | { 283 | CVect3 attH(att.i, att.j, 0.0); 284 | CVect3 magH = a2mat(attH)*mag; 285 | double yaw = 0.0; 286 | if (attH.i<(80.0*DEG) && attH.i>-(80.0*DEG)) 287 | { 288 | yaw = atan2Ex(magH.i, magH.j) + declination; 289 | if (yaw > PI) yaw -= _2PI; 290 | else if (yaw < -PI) yaw += _2PI; 291 | } 292 | return yaw; 293 | } 294 | 295 | CVect3 xyz2blh(const CVect3 &xyz) 296 | { 297 | double s = normXY(xyz), theta = atan2(xyz.k*glv.Re, s*glv.Rp), 298 | s3 = sin(theta), c3 = cos(theta); s3 = s3 * s3*s3, c3 = c3 * c3*c3; 299 | if (s < (6378137.0*1.0*DEG)) return O31; 300 | double L = atan2(xyz.j, xyz.i), B = atan2(xyz.k + glv.ep2*glv.Rp*s3, s - glv.e2*glv.Re*c3), 301 | sB = sin(B), cB = cos(B), N = glv.Re / sqrt(1 - glv.e2*sB*sB); 302 | return CVect3(B, L, s / cB - N); 303 | } 304 | 305 | CVect3 blh2xyz(const CVect3 &blh) 306 | { 307 | double sB = sin(blh.i), cB = cos(blh.i), sL = sin(blh.j), cL = cos(blh.j), 308 | N = glv.Re / sqrt(1 - glv.e2*sB*sB); 309 | return CVect3((N + blh.k)*cB*cL, (N + blh.k)*cB*sL, (N*(1 - glv.e2) + blh.k)*sB); 310 | } 311 | 312 | CVect3 Vxyz2enu(const CVect3 &Vxyz, const CVect3 &pos) 313 | { 314 | return Vxyz * pos2Cen(pos); 315 | } 316 | 317 | /*************************** class CQuat *********************************/ 318 | CQuat::CQuat(void) 319 | { 320 | } 321 | 322 | CQuat::CQuat(double qq0, double qq1, double qq2, double qq3) 323 | { 324 | q0 = qq0, q1 = qq1, q2 = qq2, q3 = qq3; 325 | } 326 | 327 | CQuat::CQuat(const double *pdata) 328 | { 329 | q0 = *pdata++, q1 = *pdata++, q2 = *pdata++, q3 = *pdata++; 330 | } 331 | 332 | CQuat CQuat::operator+(const CVect3 &phi) const 333 | { 334 | CQuat qtmp = rv2q(-phi); 335 | return qtmp * (*this); 336 | } 337 | 338 | CQuat CQuat::operator-(const CVect3 &phi) const 339 | { 340 | CQuat qtmp = rv2q(phi); 341 | return qtmp * (*this); 342 | } 343 | 344 | CVect3 CQuat::operator-(CQuat &quat) const 345 | { 346 | CQuat dq; 347 | 348 | dq = quat * (~(*this)); 349 | if (dq.q0 < 0) 350 | { 351 | dq.q0 = -dq.q0, dq.q1 = -dq.q1, dq.q2 = -dq.q2, dq.q3 = -dq.q3; 352 | } 353 | double n2 = acos(dq.q0), f; 354 | if (sign(n2) != 0) 355 | { 356 | f = 2.0 / (sin(n2) / n2); 357 | } 358 | else 359 | { 360 | f = 2.0; 361 | } 362 | return CVect3(dq.q1, dq.q2, dq.q3)*f; 363 | } 364 | 365 | CQuat CQuat::operator*(const CQuat &quat) const 366 | { 367 | CQuat qtmp; 368 | qtmp.q0 = q0 * quat.q0 - q1 * quat.q1 - q2 * quat.q2 - q3 * quat.q3; 369 | qtmp.q1 = q0 * quat.q1 + q1 * quat.q0 + q2 * quat.q3 - q3 * quat.q2; 370 | qtmp.q2 = q0 * quat.q2 + q2 * quat.q0 + q3 * quat.q1 - q1 * quat.q3; 371 | qtmp.q3 = q0 * quat.q3 + q3 * quat.q0 + q1 * quat.q2 - q2 * quat.q1; 372 | return qtmp; 373 | } 374 | 375 | CQuat& CQuat::operator*=(const CQuat &quat) 376 | { 377 | return (*this = *this*quat); 378 | } 379 | 380 | CQuat& CQuat::operator-=(const CVect3 &phi) 381 | { 382 | CQuat qtmp = rv2q(phi); 383 | return (*this = qtmp * (*this)); 384 | } 385 | 386 | CQuat operator~(const CQuat &q) 387 | { 388 | return CQuat(q.q0, -q.q1, -q.q2, -q.q3); 389 | } 390 | 391 | CVect3 CQuat::operator*(const CVect3 &v) const 392 | { 393 | CQuat qtmp; 394 | CVect3 vtmp; 395 | qtmp.q0 = -q1 * v.i - q2 * v.j - q3 * v.k; 396 | qtmp.q1 = q0 * v.i + q2 * v.k - q3 * v.j; 397 | qtmp.q2 = q0 * v.j + q3 * v.i - q1 * v.k; 398 | qtmp.q3 = q0 * v.k + q1 * v.j - q2 * v.i; 399 | vtmp.i = -qtmp.q0*q1 + qtmp.q1*q0 - qtmp.q2*q3 + qtmp.q3*q2; 400 | vtmp.j = -qtmp.q0*q2 + qtmp.q2*q0 - qtmp.q3*q1 + qtmp.q1*q3; 401 | vtmp.k = -qtmp.q0*q3 + qtmp.q3*q0 - qtmp.q1*q2 + qtmp.q2*q1; 402 | return vtmp; 403 | } 404 | 405 | void CQuat::SetYaw(double yaw) 406 | { 407 | CVect3 att = q2att(*this); 408 | att.k = yaw; 409 | *this = a2qua(att); 410 | } 411 | 412 | void normlize(CQuat *q) 413 | { 414 | double nq = sqrt(q->q0*q->q0 + q->q1*q->q1 + q->q2*q->q2 + q->q3*q->q3); 415 | q->q0 /= nq, q->q1 /= nq, q->q2 /= nq, q->q3 /= nq; 416 | } 417 | 418 | CVect3 q2rv(const CQuat &q) 419 | { 420 | CQuat dq; 421 | dq = q; 422 | if (dq.q0 < 0) { dq.q0 = -dq.q0, dq.q1 = -dq.q1, dq.q2 = -dq.q2, dq.q3 = -dq.q3; } 423 | if (dq.q0 > 1.0) dq.q0 = 1.0; 424 | double n2 = acos(dq.q0), f; 425 | if (n2 > 1.0e-20) 426 | { 427 | f = 2.0 / (sin(n2) / n2); 428 | } 429 | else 430 | { 431 | f = 2.0; 432 | } 433 | return CVect3(dq.q1, dq.q2, dq.q3)*f; 434 | } 435 | 436 | CVect3 qq2phi(const CQuat &qcalcu, const CQuat &qreal) 437 | { 438 | return q2rv(qreal*(~qcalcu)); 439 | } 440 | 441 | CQuat UpDown(const CQuat &q) 442 | { 443 | CVect3 att = q2att(q); 444 | att.i = -att.i; att.j += PI; 445 | return a2qua(att); 446 | } 447 | 448 | /*************************** class CMat3 *********************************/ 449 | CMat3::CMat3(void) 450 | { 451 | } 452 | 453 | CMat3::CMat3(double xx, double xy, double xz, 454 | double yx, double yy, double yz, 455 | double zx, double zy, double zz) 456 | { 457 | e00 = xx, e01 = xy, e02 = xz; e10 = yx, e11 = yy, e12 = yz; e20 = zx, e21 = zy, e22 = zz; 458 | } 459 | 460 | CMat3::CMat3(const CVect3 &v0, const CVect3 &v1, const CVect3 &v2) 461 | { 462 | e00 = v0.i, e01 = v0.j, e02 = v0.k; 463 | e10 = v1.i, e11 = v1.j, e12 = v1.k; 464 | e20 = v2.i, e21 = v2.j, e22 = v2.k; 465 | } 466 | 467 | CMat3 dv2att(CVect3 &va1, const CVect3 &va2, CVect3 &vb1, const CVect3 &vb2) 468 | { 469 | CVect3 a = va1 * va2, b = vb1 * vb2, aa = a * va1, bb = b * vb1; 470 | if (IsZero(va1, EPS) || IsZero(a, EPS) || IsZero(aa, EPS) || IsZero(vb1, EPS) || IsZero(b, EPS) || IsZero(bb, EPS)) return I33; 471 | CMat3 Ma(va1 / norm(va1), a / norm(a), aa / norm(aa)), Mb(vb1 / norm(vb1), b / norm(b), bb / norm(bb)); 472 | return (~Ma)*(Mb); //Cab 473 | } 474 | 475 | CMat3 operator-(const CMat3 &m) 476 | { 477 | return CMat3(-m.e00, -m.e01, -m.e02, -m.e10, -m.e11, -m.e12, -m.e20, -m.e21, -m.e22); 478 | } 479 | 480 | CMat3 operator~(const CMat3 &m) 481 | { 482 | return CMat3(m.e00, m.e10, m.e20, m.e01, m.e11, m.e21, m.e02, m.e12, m.e22); 483 | } 484 | 485 | CMat3 CMat3::operator*(const CMat3 &mat) const 486 | { 487 | CMat3 mtmp; 488 | mtmp.e00 = e00 * mat.e00 + e01 * mat.e10 + e02 * mat.e20; 489 | mtmp.e01 = e00 * mat.e01 + e01 * mat.e11 + e02 * mat.e21; 490 | mtmp.e02 = e00 * mat.e02 + e01 * mat.e12 + e02 * mat.e22; 491 | mtmp.e10 = e10 * mat.e00 + e11 * mat.e10 + e12 * mat.e20; 492 | mtmp.e11 = e10 * mat.e01 + e11 * mat.e11 + e12 * mat.e21; 493 | mtmp.e12 = e10 * mat.e02 + e11 * mat.e12 + e12 * mat.e22; 494 | mtmp.e20 = e20 * mat.e00 + e21 * mat.e10 + e22 * mat.e20; 495 | mtmp.e21 = e20 * mat.e01 + e21 * mat.e11 + e22 * mat.e21; 496 | mtmp.e22 = e20 * mat.e02 + e21 * mat.e12 + e22 * mat.e22; 497 | return mtmp; 498 | } 499 | 500 | CMat3 CMat3::operator+(const CMat3 &mat) const 501 | { 502 | CMat3 mtmp; 503 | mtmp.e00 = e00 + mat.e00; mtmp.e01 = e01 + mat.e01; mtmp.e02 = e02 + mat.e02; 504 | mtmp.e10 = e10 + mat.e10; mtmp.e11 = e11 + mat.e11; mtmp.e12 = e12 + mat.e12; 505 | mtmp.e20 = e20 + mat.e20; mtmp.e21 = e21 + mat.e21; mtmp.e22 = e22 + mat.e22; 506 | return mtmp; 507 | } 508 | 509 | CMat3& CMat3::operator+=(const CMat3 &mat) 510 | { 511 | this->e00 += mat.e00; this->e01 += mat.e01; this->e02 += mat.e02; 512 | this->e10 += mat.e10; this->e11 += mat.e11; this->e12 += mat.e12; 513 | this->e20 += mat.e20; this->e21 += mat.e21; this->e22 += mat.e22; 514 | return *this; 515 | } 516 | 517 | CMat3 CMat3::operator-(const CMat3 &mat) const 518 | { 519 | CMat3 mtmp; 520 | mtmp.e00 = e00 - mat.e00; mtmp.e01 = e01 - mat.e01; mtmp.e02 = e02 - mat.e02; 521 | mtmp.e10 = e10 - mat.e10; mtmp.e11 = e11 - mat.e11; mtmp.e12 = e12 - mat.e12; 522 | mtmp.e20 = e20 - mat.e20; mtmp.e21 = e21 - mat.e21; mtmp.e22 = e22 - mat.e22; 523 | return mtmp; 524 | } 525 | 526 | CMat3 CMat3::operator*(double f) const 527 | { 528 | return CMat3(e00*f, e01*f, e02*f, e10*f, e11*f, e12*f, e20*f, e21*f, e22*f); 529 | } 530 | 531 | CMat3 operator*(double f, const CMat3 &m) 532 | { 533 | return CMat3(m.e00*f, m.e01*f, m.e02*f, m.e10*f, m.e11*f, m.e12*f, m.e20*f, m.e21*f, m.e22*f); 534 | } 535 | 536 | CVect3 CMat3::operator*(const CVect3 &v) const 537 | { 538 | return CVect3(e00*v.i + e01 * v.j + e02 * v.k, e10*v.i + e11 * v.j + e12 * v.k, e20*v.i + e21 * v.j + e22 * v.k); 539 | } 540 | 541 | double det(const CMat3 &m) 542 | { 543 | return m.e00*(m.e11*m.e22 - m.e12*m.e21) - m.e01*(m.e10*m.e22 - m.e12*m.e20) + m.e02*(m.e10*m.e21 - m.e11*m.e20); 544 | } 545 | 546 | double trace(const CMat3 &m) 547 | { 548 | return (m.e00 + m.e11 + m.e22); 549 | } 550 | 551 | CMat3 pow(const CMat3 &m, int k) 552 | { 553 | CMat3 mm = m; 554 | for (int i = 1; i < k; i++) mm = mm * m; 555 | return mm; 556 | } 557 | 558 | CQuat a2qua(double pitch, double roll, double yaw) 559 | { 560 | pitch /= 2.0, roll /= 2.0, yaw /= 2.0; 561 | double sp = sin(pitch), sr = sin(roll), sy = sin(yaw), 562 | cp = cos(pitch), cr = cos(roll), cy = cos(yaw); 563 | CQuat qnb; 564 | qnb.q0 = cp * cr*cy - sp * sr*sy; 565 | qnb.q1 = sp * cr*cy - cp * sr*sy; 566 | qnb.q2 = cp * sr*cy + sp * cr*sy; 567 | qnb.q3 = cp * cr*sy + sp * sr*cy; 568 | return qnb; 569 | } 570 | 571 | CQuat a2qua(const CVect3 &att) 572 | { 573 | return a2qua(att.i, att.j, att.k); 574 | } 575 | 576 | CMat3 a2mat(const CVect3 &att) 577 | { 578 | double si = sin(att.i), ci = cos(att.i), 579 | sj = sin(att.j), cj = cos(att.j), 580 | sk = sin(att.k), ck = cos(att.k); 581 | CMat3 Cnb; 582 | Cnb.e00 = cj * ck - si * sj*sk; Cnb.e01 = -ci * sk; Cnb.e02 = sj * ck + si * cj*sk; 583 | Cnb.e10 = cj * sk + si * sj*ck; Cnb.e11 = ci * ck; Cnb.e12 = sj * sk - si * cj*ck; 584 | Cnb.e20 = -ci * sj; Cnb.e21 = si; Cnb.e22 = ci * cj; 585 | return Cnb; 586 | } 587 | 588 | CVect3 m2att(const CMat3 &Cnb) 589 | { 590 | CVect3 att; 591 | att.i = asinEx(Cnb.e21); 592 | att.j = atan2Ex(-Cnb.e20, Cnb.e22); 593 | att.k = atan2Ex(-Cnb.e01, Cnb.e11); 594 | return att; 595 | } 596 | 597 | CQuat m2qua(const CMat3 &Cnb) 598 | { 599 | double q0, q1, q2, q3, qq4; 600 | if (Cnb.e00 >= Cnb.e11 + Cnb.e22) 601 | { 602 | q1 = 0.5*sqrt(1 + Cnb.e00 - Cnb.e11 - Cnb.e22); qq4 = 4 * q1; 603 | q0 = (Cnb.e21 - Cnb.e12) / qq4; q2 = (Cnb.e01 + Cnb.e10) / qq4; q3 = (Cnb.e02 + Cnb.e20) / qq4; 604 | } 605 | else if (Cnb.e11 >= Cnb.e00 + Cnb.e22) 606 | { 607 | q2 = 0.5*sqrt(1 - Cnb.e00 + Cnb.e11 - Cnb.e22); qq4 = 4 * q2; 608 | q0 = (Cnb.e02 - Cnb.e20) / qq4; q1 = (Cnb.e01 + Cnb.e10) / qq4; q3 = (Cnb.e12 + Cnb.e21) / qq4; 609 | } 610 | else if (Cnb.e22 >= Cnb.e00 + Cnb.e11) 611 | { 612 | q3 = 0.5*sqrt(1 - Cnb.e00 - Cnb.e11 + Cnb.e22); qq4 = 4 * q3; 613 | q0 = (Cnb.e10 - Cnb.e01) / qq4; q1 = (Cnb.e02 + Cnb.e20) / qq4; q2 = (Cnb.e12 + Cnb.e21) / qq4; 614 | } 615 | else 616 | { 617 | q0 = 0.5*sqrt(1 + Cnb.e00 + Cnb.e11 + Cnb.e22); qq4 = 4 * q0; 618 | q1 = (Cnb.e21 - Cnb.e12) / qq4; q2 = (Cnb.e02 - Cnb.e20) / qq4; q3 = (Cnb.e10 - Cnb.e01) / qq4; 619 | } 620 | double nq = sqrt(q0*q0 + q1 * q1 + q2 * q2 + q3 * q3); 621 | q0 /= nq; q1 /= nq; q2 /= nq; q3 /= nq; 622 | return CQuat(q0, q1, q2, q3); 623 | } 624 | 625 | CVect3 q2att(const CQuat &qnb) 626 | { 627 | double q11 = qnb.q0*qnb.q0, q12 = qnb.q0*qnb.q1, q13 = qnb.q0*qnb.q2, q14 = qnb.q0*qnb.q3, 628 | q22 = qnb.q1*qnb.q1, q23 = qnb.q1*qnb.q2, q24 = qnb.q1*qnb.q3, 629 | q33 = qnb.q2*qnb.q2, q34 = qnb.q2*qnb.q3, 630 | q44 = qnb.q3*qnb.q3; 631 | CVect3 att; 632 | att.i = asinEx(2 * (q34 + q12)); 633 | att.j = atan2Ex(-2 * (q24 - q13), q11 - q22 - q33 + q44); 634 | att.k = atan2Ex(-2 * (q23 - q14), q11 - q22 + q33 - q44); 635 | return att; 636 | } 637 | 638 | CMat3 q2mat(const CQuat &qnb) 639 | { 640 | double q11 = qnb.q0*qnb.q0, q12 = qnb.q0*qnb.q1, q13 = qnb.q0*qnb.q2, q14 = qnb.q0*qnb.q3, 641 | q22 = qnb.q1*qnb.q1, q23 = qnb.q1*qnb.q2, q24 = qnb.q1*qnb.q3, 642 | q33 = qnb.q2*qnb.q2, q34 = qnb.q2*qnb.q3, 643 | q44 = qnb.q3*qnb.q3; 644 | CMat3 Cnb; 645 | Cnb.e00 = q11 + q22 - q33 - q44, Cnb.e01 = 2 * (q23 - q14), Cnb.e02 = 2 * (q24 + q13), 646 | Cnb.e10 = 2 * (q23 + q14), Cnb.e11 = q11 - q22 + q33 - q44, Cnb.e12 = 2 * (q34 - q12), 647 | Cnb.e20 = 2 * (q24 - q13), Cnb.e21 = 2 * (q34 + q12), Cnb.e22 = q11 - q22 - q33 + q44; 648 | return Cnb; 649 | } 650 | 651 | CMat3 adj(const CMat3 &m) 652 | { 653 | CMat3 mtmp; 654 | mtmp.e00 = (m.e11*m.e22 - m.e12*m.e21); 655 | mtmp.e10 = -(m.e10*m.e22 - m.e12*m.e20); 656 | mtmp.e20 = (m.e10*m.e21 - m.e11*m.e20); 657 | mtmp.e01 = -(m.e01*m.e22 - m.e02*m.e21); 658 | mtmp.e11 = (m.e00*m.e22 - m.e02*m.e20); 659 | mtmp.e21 = -(m.e00*m.e21 - m.e01*m.e20); 660 | mtmp.e02 = (m.e01*m.e12 - m.e02*m.e11); 661 | mtmp.e12 = -(m.e00*m.e12 - m.e02*m.e10); 662 | mtmp.e22 = (m.e00*m.e11 - m.e01*m.e10); 663 | return mtmp; 664 | } 665 | 666 | CMat3 inv(const CMat3 &m) 667 | { 668 | CMat3 adjm = adj(m); 669 | double detm = m.e00*adjm.e00 + m.e01*adjm.e10 + m.e02*adjm.e20; 670 | return adjm * (1.0 / detm); 671 | } 672 | 673 | CVect3 diag(const CMat3 &m) 674 | { 675 | return CVect3(m.e00, m.e11, m.e22); 676 | } 677 | 678 | CMat3 diag(const CVect3 &v) 679 | { 680 | return CMat3(v.i, 0, 0, 0, v.j, 0, 0, 0, v.k); 681 | } 682 | 683 | CMat3 foam(const CMat3 &B, int iter) 684 | { 685 | CMat3 adjBp = adj(~B), BBp = B * (~B); 686 | double detB = det(B), adjBp2 = trace(adjBp*(~adjBp)), B2 = trace(BBp), 687 | lambda = sqrt(3 * B2), lambda2, kappa, zeta, Psi, dPsi, dlambda; 688 | psinsassert(detB > 0.0); 689 | for (int k = 1; k < iter; k++) 690 | { 691 | lambda2 = lambda * lambda; 692 | kappa = (lambda2 - B2) / 2.0; 693 | zeta = kappa * lambda - detB; 694 | Psi = (lambda2 - B2); Psi = Psi * Psi - 8.0*lambda*detB - 4.0*adjBp2; 695 | dPsi = 8.0*zeta; 696 | dlambda = Psi / dPsi; 697 | lambda = lambda - dlambda; 698 | if (fabs(dlambda / lambda) < 1.0e-15) break; 699 | } 700 | return ((kappa + B2)*B + lambda * adjBp - BBp * B) * (1.0 / zeta); 701 | } 702 | 703 | /*************************** class CMat *********************************/ 704 | CMat::CMat(void) 705 | { 706 | #ifdef MAT_COUNT_STATISTIC 707 | #pragma message(" MAT_COUNT_STATISTIC") 708 | if (iMax < ++iCount) iMax = iCount; 709 | #endif 710 | } 711 | 712 | CMat::CMat(int row0, int clm0) 713 | { 714 | #ifdef MAT_COUNT_STATISTIC 715 | if (iMax < ++iCount) iMax = iCount; 716 | #endif 717 | row = row0; clm = clm0; rc = row * clm; 718 | } 719 | 720 | CMat::CMat(int row0, int clm0, double f) 721 | { 722 | #ifdef MAT_COUNT_STATISTIC 723 | if (iMax < ++iCount) iMax = iCount; 724 | #endif 725 | row = row0; clm = clm0; rc = row * clm; 726 | for (double *pd = dd, *pEnd = &dd[rc]; pd < pEnd; pd++) *pd = f; 727 | } 728 | 729 | CMat::CMat(int row0, int clm0, const double *pf) 730 | { 731 | #ifdef MAT_COUNT_STATISTIC 732 | if (iMax < ++iCount) iMax = iCount; 733 | #endif 734 | row = row0; clm = clm0; rc = row * clm; 735 | memcpy(dd, pf, rc * sizeof(double)); 736 | } 737 | 738 | #ifdef MAT_COUNT_STATISTIC 739 | int CMat::iCount = 0, CMat::iMax = 0; 740 | CMat::~CMat(void) 741 | { 742 | iCount--; 743 | } 744 | #endif 745 | 746 | void CMat::Clear(void) 747 | { 748 | for (double *p = dd, *pEnd = &dd[rc]; p < pEnd; p++) *p = 0.0; 749 | } 750 | 751 | CMat CMat::operator*(const CMat &m0) const 752 | { 753 | #ifdef MAT_COUNT_STATISTIC 754 | ++iCount; 755 | #endif 756 | psinsassert(this->clm == m0.row); 757 | CMat mtmp(this->row, m0.clm); 758 | int m = this->row, k = this->clm, n = m0.clm; 759 | double *p = mtmp.dd; const double *p1i = this->dd, *p2 = m0.dd; 760 | for (int i = 0; i < m; i++, p1i += k) 761 | { 762 | for (int j = 0; j < n; j++) 763 | { 764 | double f = 0.0; const double *p1is = p1i, *p1isEnd = &p1i[k], *p2sj = &p2[j]; 765 | for (; p1is < p1isEnd; p1is++, p2sj += n) 766 | f += (*p1is) * (*p2sj); 767 | *p++ = f; 768 | } 769 | } 770 | return mtmp; 771 | } 772 | 773 | CVect CMat::operator*(const CVect &v) const 774 | { 775 | psinsassert(this->clm == v.row); 776 | CVect vtmp(this->row); 777 | double *p = vtmp.dd, *pEnd = &vtmp.dd[vtmp.row]; const double *p1ij = this->dd, *p2End = &v.dd[v.row]; 778 | int iii=0; 779 | for (; p < pEnd; p++) 780 | { 781 | double f = 0.0; const double *p2j = v.dd; 782 | for (; p2j < p2End; p1ij++, p2j++) 783 | f += (*p1ij) * (*p2j); 784 | *p = f; 785 | iii++; 786 | if (iii==17) 787 | { 788 | iii=17; 789 | } 790 | } 791 | return vtmp; 792 | } 793 | 794 | CMat CMat::operator+(const CMat &m0) const 795 | { 796 | #ifdef MAT_COUNT_STATISTIC 797 | ++iCount; 798 | #endif 799 | psinsassert(row == m0.row&&clm == m0.clm); 800 | CMat mtmp(row, clm); 801 | double *p = mtmp.dd, *pEnd = &mtmp.dd[rc]; const double *p1 = this->dd, *p2 = m0.dd; 802 | while (p < pEnd) 803 | { 804 | *p++ = (*p1++) + (*p2++); 805 | } 806 | return mtmp; 807 | } 808 | 809 | CMat& CMat::operator+=(const CVect &v) 810 | { 811 | psinsassert(row == v.row || clm == v.clm); 812 | int row1 = row + 1; 813 | double *p = dd, *pEnd = &dd[rc]; 814 | for (const double *p1 = v.dd; p < pEnd; p += row1, p1++) *p += *p1; 815 | return *this; 816 | } 817 | 818 | CMat CMat::operator-(const CMat &m0) const 819 | { 820 | #ifdef MAT_COUNT_STATISTIC 821 | ++iCount; 822 | #endif 823 | psinsassert(row == m0.row&&clm == m0.clm); 824 | CMat mtmp(row, clm); 825 | double *p = mtmp.dd, *pEnd = &mtmp.dd[rc]; const double *p1 = this->dd, *p2 = m0.dd; 826 | while (p < pEnd) 827 | { 828 | *p++ = (*p1++) - (*p2++); 829 | } 830 | return mtmp; 831 | } 832 | 833 | CMat CMat::operator*(double f) const 834 | { 835 | #ifdef MAT_COUNT_STATISTIC 836 | ++iCount; 837 | #endif 838 | CMat mtmp(row, clm); 839 | double *p = mtmp.dd, *pEnd = &mtmp.dd[rc]; const double *p1 = this->dd; 840 | while (p < pEnd) 841 | { 842 | *p++ = (*p1++) * f; 843 | } 844 | return mtmp; 845 | } 846 | 847 | CMat& CMat::operator+=(const CMat &m0) 848 | { 849 | psinsassert(row == m0.row&&clm == m0.clm); 850 | double *p = dd, *pEnd = &dd[rc]; const double *p1 = m0.dd; 851 | while (p < pEnd) 852 | { 853 | *p++ += *p1++; 854 | } 855 | return *this; 856 | } 857 | 858 | CMat& CMat::operator-=(const CMat &m0) 859 | { 860 | psinsassert(row == m0.row&&clm == m0.clm); 861 | double *p = dd, *pEnd = &dd[rc]; const double *p1 = m0.dd; 862 | while (p < pEnd) 863 | { 864 | *p++ -= *p1++; 865 | } 866 | return *this; 867 | } 868 | 869 | CMat& CMat::operator*=(double f) 870 | { 871 | double *p = dd, *pEnd = &dd[rc]; 872 | while (p < pEnd) 873 | { 874 | *p++ *= f; 875 | } 876 | return *this; 877 | } 878 | 879 | CMat& CMat::operator++() 880 | { 881 | int row1 = row + 1; 882 | for (double *p = dd, *pEnd = &dd[rc]; p < pEnd; p += row1) *p += 1.0; 883 | return *this; 884 | } 885 | 886 | CMat operator~(const CMat &m0) 887 | { 888 | #ifdef MAT_COUNT_STATISTIC 889 | ++CMat::iCount; 890 | #endif 891 | CMat mtmp(m0.clm, m0.row); 892 | const double *pm = m0.dd; 893 | for (int i = 0; i < m0.row; i++) 894 | { 895 | for (int j = i; j < m0.rc; j += m0.row) mtmp.dd[j] = *pm++; 896 | } 897 | return mtmp; 898 | } 899 | 900 | void symmetry(CMat &m) 901 | { 902 | psinsassert(m.row == m.clm); 903 | double *prow0 = &m.dd[1], *prowEnd = &m.dd[m.clm], *pclm0 = &m.dd[m.clm], *pEnd = &m.dd[m.rc]; 904 | for (int clm1 = m.clm + 1; prow0 < pEnd; prow0 += clm1, pclm0 += clm1, prowEnd += m.clm) 905 | { 906 | for (double *prow = prow0, *pclm = pclm0; prow < prowEnd; prow++, pclm += m.clm) 907 | *prow = *pclm = (*prow + *pclm)*0.5; 908 | } 909 | } 910 | 911 | double& CMat::operator()(int r, int c) 912 | { 913 | if (c < 0) c = r; 914 | return this->dd[r*this->clm + c]; 915 | } 916 | 917 | void CMat::SetRow(int i, double f, ...) 918 | { 919 | va_list vl; 920 | va_start(vl, f); 921 | for (double *p = &dd[i*clm], *pEnd = p + clm; p < pEnd; p++) 922 | { 923 | *p = f; f = va_arg(vl, double); 924 | } 925 | va_end(vl); 926 | return; 927 | } 928 | 929 | void CMat::SetRow(int i, const CVect &v) 930 | { 931 | psinsassert(clm == v.clm); 932 | const double *p = v.dd; 933 | for (double *p1 = &dd[i*clm], *pEnd = p1 + clm; p1 < pEnd; p++, p1++) *p1 = *p; 934 | return; 935 | } 936 | 937 | void CMat::SetClm(int j, double f, ...) 938 | { 939 | va_list vl; 940 | va_start(vl, f); 941 | for (double *p = &dd[j], *pEnd = &p[rc]; p < pEnd; p += clm) 942 | { 943 | *p = f; f = va_arg(vl, double); 944 | } 945 | va_end(vl); 946 | return; 947 | } 948 | 949 | void CMat::SetClm(int j, const CVect &v) 950 | { 951 | psinsassert(row == v.row); 952 | const double *p = v.dd; 953 | for (double *p1 = &dd[j], *pEnd = &dd[rc]; p1 < pEnd; p++, p1 += clm) *p1 = *p; 954 | return; 955 | } 956 | 957 | void CMat::SetClmVect3(int i, int j, const CVect3 &v) 958 | { 959 | double *p = &dd[i*clm + j]; 960 | *p = v.i; p += clm; 961 | *p = v.j; p += clm; 962 | *p = v.k; 963 | } 964 | 965 | void CMat::SetRowVect3(int i, int j, const CVect3 &v) 966 | { 967 | *(CVect3*)&dd[i*clm + j] = v; 968 | } 969 | 970 | void CMat::SetDiagVect3(int i, int j, const CVect3 &v) 971 | { 972 | double *p = &dd[i*clm + j]; 973 | *p = v.i; p += clm + 1; 974 | *p = v.j; p += clm + 1; 975 | *p = v.k; 976 | } 977 | 978 | CVect3 CMat::GetDiagVect3(int i, int j) 979 | { 980 | if (j == -1) j = i; 981 | CVect3 v; 982 | double *p = &dd[i*clm + j]; 983 | v.i = *p; p += clm + 1; 984 | v.j = *p; p += clm + 1; 985 | v.k = *p; 986 | return v; 987 | } 988 | 989 | void CMat::SetAskew(int i, int j, const CVect3 &v) 990 | { 991 | double *p = &dd[i*clm + j]; 992 | p[0] = 0.0; p[1] = -v.k; p[2] = v.j; p += clm; 993 | p[0] = v.k; p[1] = 0.0; p[2] = -v.i; p += clm; 994 | p[0] = -v.j; p[1] = v.i; p[2] = 0.0; 995 | } 996 | 997 | void CMat::SetMat3(int i, int j, const CMat3 &m) 998 | { 999 | double *p = &dd[i*clm + j]; 1000 | *(CVect3*)p = *(CVect3*)&m.e00; p += clm; 1001 | *(CVect3*)p = *(CVect3*)&m.e10; p += clm; 1002 | *(CVect3*)p = *(CVect3*)&m.e20; 1003 | } 1004 | 1005 | CMat3 CMat::GetMat3(int i, int j) const 1006 | { 1007 | if (j = -1) j = i; 1008 | CMat3 m; 1009 | const double *p = &dd[i*clm + j]; 1010 | *(CVect3*)&m.e00 = *(CVect3*)p; p += clm; 1011 | *(CVect3*)&m.e10 = *(CVect3*)p; p += clm; 1012 | *(CVect3*)&m.e20 = *(CVect3*)p; 1013 | return m; 1014 | } 1015 | 1016 | void CMat::SubAddMat3(int i, int j, const CMat3 &m) 1017 | { 1018 | double *p = &dd[i*clm + j]; 1019 | *(CVect3*)p += *(CVect3*)&m.e00; p += clm; 1020 | *(CVect3*)p += *(CVect3*)&m.e10; p += clm; 1021 | *(CVect3*)p += *(CVect3*)&m.e20; 1022 | } 1023 | 1024 | CVect CMat::GetRow(int i) const 1025 | { 1026 | CVect v(1, clm); 1027 | const double *p1 = &dd[i*clm], *pEnd = p1 + clm; 1028 | for (double *p = v.dd; p1 < pEnd; p++, p1++) *p = *p1; 1029 | return v; 1030 | } 1031 | 1032 | CVect CMat::GetClm(int j) const 1033 | { 1034 | CVect v(row, 1); 1035 | const double *p1 = &dd[j], *pEnd = &dd[rc]; 1036 | for (double *p = v.dd; p1 < pEnd; p++, p1 += clm) *p = *p1; 1037 | return v; 1038 | } 1039 | 1040 | void CMat::ZeroRow(int i) 1041 | { 1042 | for (double *p = &dd[i*clm], *pEnd = p + clm; p < pEnd; p++) *p = 0.0; 1043 | return; 1044 | } 1045 | 1046 | void CMat::ZeroClm(int j) 1047 | { 1048 | for (double *p = &dd[j], *pEnd = &dd[rc]; p < pEnd; p += clm) *p = 0.0; 1049 | return; 1050 | } 1051 | 1052 | void CMat::SetDiag(double f, ...) 1053 | { 1054 | *this = CMat(this->row, this->clm, 0.0); 1055 | va_list vl; 1056 | va_start(vl, f); 1057 | double *p = dd, *pEnd = &dd[rc]; 1058 | for (int row1 = row + 1; p < pEnd; p += row1) 1059 | { 1060 | *p = f; f = va_arg(vl, double); 1061 | } 1062 | va_end(vl); 1063 | } 1064 | 1065 | void CMat::SetDiag2(double f, ...) 1066 | { 1067 | *this = CMat(this->row, this->clm, 0.0); 1068 | va_list vl; 1069 | va_start(vl, f); 1070 | double *p = dd, *pEnd = &dd[rc]; 1071 | for (int row1 = row + 1; p < pEnd; p += row1) 1072 | { 1073 | *p = f * f; f = va_arg(vl, double); 1074 | } 1075 | va_end(vl); 1076 | } 1077 | 1078 | double normInf(CMat &m) 1079 | { 1080 | double n1 = 0.0; 1081 | for (double *p = m.dd, *pEnd = &m.dd[m.rc]; p < pEnd; p++) 1082 | { 1083 | if (*p > n1) n1 = *p; 1084 | else if (-*p > n1) n1 = -*p; 1085 | } 1086 | return n1; 1087 | } 1088 | 1089 | CVect diag(const CMat &m) 1090 | { 1091 | int row1 = m.row + 1; 1092 | CVect vtmp(m.row, 1); 1093 | double *p = vtmp.dd, *pEnd = &vtmp.dd[vtmp.row]; 1094 | for (const double *p1 = m.dd; p < pEnd; p++, p1 += row1) *p = *p1; 1095 | return vtmp; 1096 | } 1097 | 1098 | void RowMul(CMat &m, const CMat &m0, const CMat &m1, int r) 1099 | { 1100 | psinsassert(m0.clm == m1.row); 1101 | int rc0 = r * m0.clm; 1102 | double *p = &m.dd[rc0], *pEnd = p + m0.clm; const double *p0 = &m0.dd[rc0], *p0End = p0 + m0.clm, *p1j = m1.dd; 1103 | for (; p < pEnd; p++) 1104 | { 1105 | double f = 0.0; const double *p0j = p0, *p1jk = p1j++; 1106 | for (; p0j < p0End; p0j++, p1jk += m1.clm) f += (*p0j) * (*p1jk); 1107 | *p = f; 1108 | } 1109 | } 1110 | 1111 | void RowMulT(CMat &m, const CMat &m0, const CMat &m1, int r) 1112 | { 1113 | psinsassert(m0.clm == m1.clm); 1114 | int rc0 = r * m0.clm; 1115 | double *p = &m.dd[rc0], *pEnd = p + m0.clm; const double *p0 = &m0.dd[rc0], *p0End = p0 + m0.clm, *p1jk = m1.dd; 1116 | for (; p < pEnd; p++) 1117 | { 1118 | double f = 0.0; const double *p0j = p0; 1119 | for (; p0j < p0End; p0j++, p1jk++) f += (*p0j) * (*p1jk); 1120 | *p = f; 1121 | } 1122 | } 1123 | 1124 | CMat diag(const CVect &v) 1125 | { 1126 | #ifdef MAT_COUNT_STATISTIC 1127 | ++CMat::iCount; 1128 | #endif 1129 | int rc = v.row > v.clm ? v.row : v.clm, rc1 = rc + 1; 1130 | CMat mtmp(rc, rc, 0.0); 1131 | double *p = mtmp.dd; 1132 | for (const double *p1 = v.dd, *p1End = &v.dd[rc]; p1 < p1End; p += rc1, p1++) *p = *p1; 1133 | return mtmp; 1134 | } 1135 | 1136 | void DVMDVafa(const CVect &V, CMat &M, double afa) 1137 | { 1138 | psinsassert(V.rc == M.row&&M.row == M.clm); 1139 | int i = 0; 1140 | const double *pv = V.dd; 1141 | for (double vi = *pv, viafa = vi * afa; i < M.clm; i++, pv++, vi = *pv, viafa = vi * afa) 1142 | { 1143 | for (double *prow = &M.dd[i*M.clm], *prowEnd = prow + M.clm, *pclm = &M.dd[i]; prow < prowEnd; prow++, pclm += M.row) 1144 | { 1145 | *prow *= vi; 1146 | *pclm *= viafa; 1147 | } 1148 | } 1149 | } 1150 | 1151 | /*************************** class CVect *********************************/ 1152 | CVect::CVect(void) 1153 | { 1154 | } 1155 | 1156 | CVect::CVect(int row0, int clm0) 1157 | { 1158 | if (clm0 == 1) { row = row0; clm = 1; } 1159 | else { row = 1; clm = clm0; } 1160 | rc = row * clm; 1161 | } 1162 | 1163 | CVect::CVect(int row0, double f) 1164 | { 1165 | row = row0; clm = 1; rc = row * clm; 1166 | for (int i = 0; i < row; i++) dd[i] = f; 1167 | } 1168 | 1169 | CVect::CVect(int row0, const double *pf) 1170 | { 1171 | row = row0; clm = 1; rc = row * clm; 1172 | memcpy(dd, pf, row * sizeof(double)); 1173 | } 1174 | 1175 | CVect::CVect(int row0, double f, double f1, ...) 1176 | { 1177 | row = row0; clm = 1; rc = row * clm; 1178 | psinsassert(row <= MMD && clm <= MMD); 1179 | va_list vl; 1180 | va_start(vl, f); 1181 | for (int i = 0, rc = row > clm ? row : clm; i < rc; i++) 1182 | { 1183 | dd[i] = f; f = va_arg(vl, double); 1184 | } 1185 | va_end(vl); 1186 | } 1187 | 1188 | CVect::CVect(const CVect3 &v) 1189 | { 1190 | row = 3; clm = 1; rc = row * clm; 1191 | dd[0] = v.i; dd[1] = v.j; dd[2] = v.k; 1192 | } 1193 | 1194 | CVect::CVect(const CVect3 &v1, const CVect3 v2) 1195 | { 1196 | row = 6; clm = 1; rc = row * clm; 1197 | dd[0] = v1.i; dd[1] = v1.j; dd[2] = v1.k; 1198 | dd[3] = v2.i; dd[4] = v2.j; dd[5] = v2.k; 1199 | } 1200 | 1201 | CVect operator~(const CVect &v) 1202 | { 1203 | CVect vtmp = v; 1204 | vtmp.row = v.clm; vtmp.clm = v.row; 1205 | return vtmp; 1206 | } 1207 | 1208 | CVect CVect::operator*(const CMat &m) const 1209 | { 1210 | psinsassert(clm == m.row); 1211 | CVect vtmp(row, clm); 1212 | double *p = vtmp.dd; const double *p1End = &dd[clm]; 1213 | for (int j = 0; j < clm; p++, j++) 1214 | { 1215 | double f = 0.0; const double *p1j = dd, *p2jk = &m.dd[j]; 1216 | for (; p1j < p1End; p1j++, p2jk += m.clm) f += (*p1j) * (*p2jk); 1217 | *p = f; 1218 | } 1219 | return vtmp; 1220 | } 1221 | 1222 | CMat CVect::operator*(const CVect &v) const 1223 | { 1224 | #ifdef MAT_STATISTIC 1225 | ++CMat::iCount; 1226 | #endif 1227 | psinsassert(clm == v.row); 1228 | CMat mtmp(row, v.clm); 1229 | if (row == 1 && v.clm == 1) // (1x1) = (1xn)*(nx1) 1230 | { 1231 | double f = 0.0; 1232 | for (int i = 0; i < clm; i++) f += dd[i] * v.dd[i]; 1233 | mtmp.dd[0] = f; 1234 | } 1235 | else // (nxn) = (nx1)*(1xn) 1236 | { 1237 | double *p = mtmp.dd; 1238 | for (const double *p1 = &dd[0], *p1End = &dd[rc], *p2End = &v.dd[rc]; p1 < p1End; p1++) 1239 | { 1240 | for (const double *p2 = &v.dd[0]; p2 < p2End; p2++) *p++ = *p1 * *p2; 1241 | } 1242 | } 1243 | return mtmp; 1244 | } 1245 | 1246 | CVect CVect::operator+(const CVect &v) const 1247 | { 1248 | psinsassert(row == v.row&&clm == v.clm); 1249 | const double *p2 = v.dd, *p1 = dd, *p1End = &dd[rc]; 1250 | CVect vtmp(row, clm); 1251 | for (double *p = vtmp.dd; p1 < p1End; p++, p1++, p2++) { *p = *p1 + *p2; } 1252 | return vtmp; 1253 | } 1254 | 1255 | CVect CVect::operator-(const CVect &v) const 1256 | { 1257 | psinsassert(row == v.row&&clm == v.clm); 1258 | const double *p2 = v.dd, *p1 = dd, *p1End = &dd[rc]; 1259 | CVect vtmp(row, clm); 1260 | for (double *p = vtmp.dd; p1 < p1End; p++, p1++, p2++) { *p = *p1 - *p2; } 1261 | return vtmp; 1262 | } 1263 | 1264 | CVect CVect::operator*(double f) const 1265 | { 1266 | CVect vtmp(row, clm); 1267 | const double *p1 = dd, *p1End = &dd[rc]; 1268 | for (double *p = vtmp.dd; p1 < p1End; p++, p1++) { *p = *p1*f; } 1269 | return vtmp; 1270 | } 1271 | 1272 | CVect& CVect::operator=(double f) 1273 | { 1274 | for (double *p = dd, *pEnd = &dd[rc]; p < pEnd; p++) { *p = f; } 1275 | return *this; 1276 | } 1277 | 1278 | CVect& CVect::operator=(const double *pf) 1279 | { 1280 | for (double *p = dd, *pEnd = &dd[rc]; p < pEnd; p++, pf++) { *p = *pf; } 1281 | return *this; 1282 | } 1283 | 1284 | CVect& CVect::operator+=(const CVect &v) 1285 | { 1286 | psinsassert(row == v.row&&clm == v.clm); 1287 | const double *p1 = v.dd; 1288 | for (double *p = dd, *pEnd = &dd[rc]; p < pEnd; p++, p1++) { *p += *p1; } 1289 | return *this; 1290 | } 1291 | 1292 | CVect& CVect::operator-=(const CVect &v) 1293 | { 1294 | psinsassert(row == v.row&&clm == v.clm); 1295 | const double *p1 = v.dd; 1296 | for (double *p = dd, *pEnd = &dd[rc]; p < pEnd; p++, p1++) { *p -= *p1; } 1297 | return *this; 1298 | } 1299 | 1300 | CVect& CVect::operator*=(double f) 1301 | { 1302 | for (double *p = dd, *pEnd = &dd[rc]; p < pEnd; p++) { *p *= f; } 1303 | return *this; 1304 | } 1305 | 1306 | CVect pow(const CVect &v, int k) 1307 | { 1308 | CVect pp = v; 1309 | double *p, *pEnd = &pp.dd[pp.rc]; 1310 | for (int i = 1; i < k; i++) 1311 | { 1312 | p = pp.dd; 1313 | for (const double *p1 = v.dd; p < pEnd; p++, p1++) 1314 | *p *= *p1; 1315 | } 1316 | return pp; 1317 | } 1318 | 1319 | CVect abs(const CVect &v) 1320 | { 1321 | CVect res(v.row, v.clm); 1322 | const double *p = v.dd, *pEnd = &v.dd[v.rc]; 1323 | for (double *p1 = res.dd; p < pEnd; p++, p1++) { *p1 = *p > 0 ? *p : -*p; } 1324 | return res; 1325 | } 1326 | 1327 | double norm(const CVect &v) 1328 | { 1329 | const double *p = v.dd, *pEnd = &v.dd[v.rc]; 1330 | double f = 0.0; 1331 | for (; p < pEnd; p++) { f += (*p)*(*p); } 1332 | return sqrt(f); 1333 | } 1334 | 1335 | double normInf(const CVect &v) 1336 | { 1337 | const double *p = v.dd, *pEnd = &v.dd[v.rc]; 1338 | double f = 0.0; 1339 | for (; p < pEnd; p++) { if (*p > f) f = *p; else if (-*p > f) f = -*p; } 1340 | return f; 1341 | } 1342 | 1343 | double& CVect::operator()(int r) 1344 | { 1345 | return this->dd[r]; 1346 | } 1347 | 1348 | void CVect::Set(double f, ...) 1349 | { 1350 | psinsassert(rc <= MMD); 1351 | va_list vl; 1352 | va_start(vl, f); 1353 | for (int i = 0; i < rc; i++) 1354 | { 1355 | dd[i] = f; f = va_arg(vl, double); 1356 | } 1357 | va_end(vl); 1358 | } 1359 | 1360 | void CVect::Set2(double f, ...) 1361 | { 1362 | psinsassert(rc <= MMD); 1363 | va_list vl; 1364 | va_start(vl, f); 1365 | for (int i = 0; i < rc; i++) 1366 | { 1367 | dd[i] = f * f; f = va_arg(vl, double); 1368 | } 1369 | va_end(vl); 1370 | } 1371 | 1372 | /*************************** class CIIR *********************************/ 1373 | CIIR::CIIR(void) 1374 | { 1375 | } 1376 | 1377 | CIIR::CIIR(double *b0, double *a0, int n0) 1378 | { 1379 | psinsassert(n0 < IIRnMax); 1380 | for (int i = 0; i < n0; i++) { b[i] = b0[i] / a0[0]; a[i] = a0[i]; x[i] = y[i] = 0.0; } 1381 | n = n0; 1382 | } 1383 | 1384 | double CIIR::Update(double x0) 1385 | { 1386 | // a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb) 1387 | // - a(2)*y(n-1) - ... - a(na+1)*y(n-na) 1388 | double y0 = 0.0; 1389 | for (int i = n - 1; i > 0; i--) 1390 | { 1391 | x[i] = x[i - 1]; y[i] = y[i - 1]; 1392 | y0 += b[i] * x[i] - a[i] * y[i]; 1393 | } 1394 | x[0] = x0; 1395 | y0 += b[0] * x0; 1396 | y[0] = y0; 1397 | return y0; 1398 | } 1399 | 1400 | /*************************** class CV3IIR *********************************/ 1401 | CIIRV3::CIIRV3(void) 1402 | { 1403 | } 1404 | 1405 | CIIRV3::CIIRV3(double *b0, double *a0, int n0, double *b1, double *a1, int n1, 1406 | double *b2, double *a2, int n2) 1407 | { 1408 | iir0 = CIIR(b0, a0, n0); 1409 | if (n1 == 0) iir1 = iir0; // iir1 the same as iir0 1410 | else iir1 = CIIR(b1, a1, n1); 1411 | if (n2 == 0) iir2 = iir0; // iir2 the same as iir0 1412 | else iir2 = CIIR(b2, a2, n2); 1413 | } 1414 | 1415 | CVect3 CIIRV3::Update(CVect3 &x) 1416 | { 1417 | return y = CVect3(iir0.Update(x.i), iir1.Update(x.j), iir2.Update(x.k)); 1418 | } 1419 | 1420 | /*************************** class CRAvar *********************************/ 1421 | CRAvar::CRAvar() 1422 | { 1423 | } 1424 | 1425 | CRAvar::CRAvar(int nR0, int maxCount0) 1426 | { 1427 | psinsassert(nR0 < RAMAX); 1428 | this->nR0 = nR0; 1429 | for (int i = 0; i < RAMAX; i++) { Rmaxcount[i] = maxCount0, tau[i] = INF; } 1430 | } 1431 | 1432 | void CRAvar::set(double r0, double tau, double rmax, double rmin, int i) 1433 | { 1434 | this->R0[i] = r0 * r0; 1435 | this->tau[i] = tau; 1436 | this->r0[i] = 0.0; Rmaxflag[i] = Rmaxcount[i]; 1437 | this->Rmax[i] = rmax == 0.0 ? 100.0*this->R0[i] : rmax * rmax; 1438 | this->Rmin[i] = rmin == 0.0 ? 0.01*this->R0[i] : rmin * rmin; 1439 | } 1440 | 1441 | void CRAvar::set(const CVect3 &r0, const CVect3 &tau, const CVect3 &rmax, const CVect3 &rmin) 1442 | { 1443 | const double *pr0 = &r0.i, *ptau = &tau.i, *prmax = &rmax.i, *prmin = &rmin.i; 1444 | for (int i = 0; i < 3; i++, pr0++, ptau++, prmax++, prmin++) 1445 | set(*pr0, *ptau, *prmax, *prmin, i); 1446 | } 1447 | 1448 | void CRAvar::set(const CVect &r0, const CVect &tau, const CVect &rmax, const CVect &rmin) 1449 | { 1450 | const double *pr0 = r0.dd, *ptau = tau.dd, *prmax = rmax.dd, *prmin = rmin.dd; 1451 | for (int i = 0; i < nR0; i++, pr0++, ptau++, prmax++, prmin++) 1452 | set(*pr0, *ptau, *prmax, *prmin, i); 1453 | } 1454 | 1455 | void CRAvar::Update(double r, double ts, int i) 1456 | { 1457 | if (tau[i] > INF / 2) return; 1458 | double tstau = ts > tau[i] ? 1.0 : ts / tau[i]; 1459 | double dr2 = r - r0[i]; dr2 = dr2 * dr2; r0[i] = r; 1460 | if (dr2 > R0[i]) R0[i] = dr2; else R0[i] = (1.0 - tstau)*R0[i] + tstau * dr2; 1461 | if (R0[i] < Rmin[i]) R0[i] = Rmin[i]; 1462 | if (R0[i] > Rmax[i]) { R0[i] = Rmax[i]; Rmaxflag[i] = Rmaxcount[i]; } 1463 | else { Rmaxflag[i] -= Rmaxflag[i] > 0; } 1464 | } 1465 | 1466 | void CRAvar::Update(const CVect3 &r, double ts) 1467 | { 1468 | const double *pr = &r.i; 1469 | for (int i = 0; i < 3; i++, pr++) 1470 | Update(*pr, ts, i); 1471 | } 1472 | 1473 | void CRAvar::Update(const CVect &r, double ts) 1474 | { 1475 | const double *pr = r.dd; 1476 | for (int i = 0; i < nR0; i++, pr++) 1477 | Update(*pr, ts, i); 1478 | } 1479 | 1480 | double CRAvar::operator()(int k) 1481 | { 1482 | return Rmaxflag[k] ? INF : sqrt(R0[k]); 1483 | } 1484 | 1485 | /*************************** class CVAR ***********************************/ 1486 | CVAR::CVAR(int imax0, float data0) 1487 | { 1488 | imax = min(imax0, VARMAX); 1489 | for (ipush = 0; ipush < imax; ipush++) array[ipush] = data0; 1490 | ipush = 0; 1491 | mean = data0; var = 0.0; 1492 | } 1493 | 1494 | float CVAR::Update(float data, BOOL isvar) 1495 | { 1496 | array[ipush] = data; 1497 | if (++ipush == imax) ipush = 0; 1498 | float *p0, *p1; 1499 | for (mean = 0.0, p0 = &array[0], p1 = &array[imax]; p0 < p1; p0++) mean += *p0; 1500 | mean /= imax; 1501 | if (isvar) 1502 | { 1503 | for (var = 0.0, p0 = &array[0], p1 = &array[imax]; p0 < p1; p0++) { float vi = *p0 - mean; var += vi * vi; } 1504 | var /= imax - 1; 1505 | } 1506 | return var; 1507 | } 1508 | 1509 | /*************************** class CVARn ***********************************/ 1510 | CVARn::CVARn(void) 1511 | { 1512 | pData = NULL; 1513 | } 1514 | 1515 | CVARn::CVARn(int row0, int clm0) 1516 | { 1517 | row = row0, clm = clm0; 1518 | pData = new double*[clm]; 1519 | pData[0] = new double[row*clm + 5 * clm]; 1520 | for (int i = 1; i < clm; i++) { 1521 | pData[i] = pData[i - 1] + row; 1522 | } 1523 | pd = pData[clm - 1] + row; Sx = pd + clm; Sx2 = Sx + clm; mx = Sx2 + clm; stdx = mx + clm; 1524 | stdsf = sqrt((row - 1) / (row - 2.0)); 1525 | Reset(); 1526 | } 1527 | 1528 | CVARn::~CVARn(void) 1529 | { 1530 | if (pData) { 1531 | // delete pData[0]; ? 1532 | delete pData; pData = NULL; 1533 | } 1534 | } 1535 | 1536 | void CVARn::Reset(void) 1537 | { 1538 | idxpush = rowcnt = 0; 1539 | memset(pData[0], 0, row*clm * sizeof(double)); 1540 | memset(Sx, 0, 5 * clm * sizeof(double)); 1541 | } 1542 | 1543 | BOOL CVARn::Update(const double *pf) 1544 | { 1545 | if (!pData[0]) return FALSE; 1546 | if (++rowcnt > row) rowcnt = row; 1547 | int idxnext = (idxpush >= row - 1) ? 0 : idxpush + 1; 1548 | for (int i = 0; i < clm; i++) 1549 | { 1550 | double f = *pf++; if (f > 1e5) f = 1e5; else if (f < -1e5) f = -1e5; 1551 | pData[i][idxpush] = f; 1552 | Sx[i] += f - pData[i][idxnext]; 1553 | Sx2[i] += f * f - pData[i][idxnext] * pData[i][idxnext]; 1554 | mx[i] = Sx[i] / rowcnt; 1555 | stdx[i] = sqrt(Sx2[i] / rowcnt - mx[i] * mx[i]) * stdsf; // Dx = E(x^2) - (Ex)^2 1556 | } 1557 | if (++idxpush == row) { 1558 | idxpush = 0; 1559 | } 1560 | return idxpush == 0; 1561 | } 1562 | 1563 | BOOL CVARn::Update(double f, ...) 1564 | { 1565 | va_list vl; 1566 | va_start(vl, f); 1567 | for (int i = 0; i < clm; i++) 1568 | { 1569 | pd[i] = f; 1570 | f = va_arg(vl, double); 1571 | } 1572 | va_end(vl); 1573 | return Update(pd); 1574 | } 1575 | 1576 | /*************************** class CKalman *********************************/ 1577 | CKalman::CKalman(int nq0, int nr0) 1578 | { 1579 | psinsassert(nq0 <= MMD && nr0 <= MMD); 1580 | kftk = 0.0; 1581 | nq = nq0; nr = nr0; 1582 | Ft = Pk = CMat(nq, nq, 0.0); 1583 | Hk = CMat(nr, nq, 0.0); Fading = CMat(nr, nq, 1.0); zfdafa = 0.1; 1584 | Qt = Pmin = Xk = CVect(nq, 0.0); Xmax = Pmax = CVect(nq, INF); Pset = CVect(nq, -1); 1585 | Zk = CVect(nr, 0.0); Rt = CVect(nr, INF); rts = CVect(nr, 1.0); Zfd = CVect(nr, 0.0); Zfd0 = CVect(nr, INF); 1586 | RtTau = Rmax = CVect(nr, INF); measlost = Rmin = Rb = CVect(nr, 0.0); Rbeta = CVect(nr, 1.0); 1587 | for (int i = 0; i < nr; i++) { Rmaxcount[i] = 0, Rmaxcount0[i] = 5; } 1588 | FBTau = FBMax = FBOne = CVect(nq, INF); FBXk = FBTotal = CVect(nq, 0.0); 1589 | measflag = measflaglog = measstop = 0; 1590 | } 1591 | 1592 | void CKalman::TimeUpdate(double kfts, int fback) 1593 | { 1594 | /* CMat Fk, FtPk; 1595 | kftk += kfts; 1596 | SetFt(nq); 1597 | Fk=++(Ft*kfts); // Fk = I+Ft*ts 1598 | Xk = Fk * Xk; 1599 | FtPk = Ft*Pk*kfts; 1600 | Pk = Pk + FtPk + (~FtPk);// + FtPk*(~Ft)*kfts; 1601 | Pk += Qt*kfts; 1602 | if(fback) Feedback(kfts); 1603 | */ 1604 | CMat Fk; 1605 | kftk += kfts; 1606 | SetFt(nq); 1607 | Fk = ++(Ft*kfts); // Fk = I+Ft*ts 1608 | Xk = Fk * Xk; 1609 | Pk = Fk * Pk*(~Fk); Pk += Qt * kfts; 1610 | if (fback) Feedback(kfts); 1611 | if (measstop > -1000000) measstop--; 1612 | } 1613 | 1614 | void CKalman::SetMeasFlag(int flag) 1615 | { 1616 | measflag = (flag == 0) ? 0 : (measflag | flag); 1617 | } 1618 | 1619 | int CKalman::MeasUpdate(double fading) 1620 | { 1621 | CVect Pxz, Kk, Hi; 1622 | SetMeas(); 1623 | if (measstop > 0) measflag = 0; 1624 | for (int i = 0; i < nr; i++) 1625 | { 1626 | if (measflag&(0x01 << i)) 1627 | { 1628 | Hi = Hk.GetRow(i); 1629 | Pxz = Pk * (~Hi); 1630 | double Pz0 = (Hi*Pxz)(0, 0), r = Zk(i) - (Hi*Xk)(0, 0); 1631 | if (Rb.dd[i] > EPS) 1632 | RAdaptive(i, r, Pz0); 1633 | if (Zfd.dd[i] < INF / 2) 1634 | RPkFading(i); 1635 | double Pzz = Pz0 + Rt.dd[i] / rts.dd[i]; 1636 | Kk = Pxz * (1.0 / Pzz); 1637 | Xk += Kk * r; 1638 | Pk -= Kk * (~Pxz); 1639 | } 1640 | } 1641 | if (fading > 1.0) Pk *= fading; 1642 | XPConstrain(); 1643 | symmetry(Pk); 1644 | int measres = measflag; 1645 | measflaglog |= measres; 1646 | SetMeasFlag(0); 1647 | return measres; 1648 | } 1649 | 1650 | int CKalman::RAdaptive(int i, double r, double Pr) 1651 | { 1652 | double rr = r * r - Pr; 1653 | if (rr < Rmin.dd[i]) rr = Rmin.dd[i]; 1654 | // if(rr>Rmax.dd[i]) Rt.dd[i] = Rmax.dd[i]; 1655 | // else Rt.dd[i] = (1.0-Rbeta.dd[i])*Rt.dd[i]+Rbeta.dd[i]*rr; 1656 | if (rr > Rmax.dd[i]) { Rt.dd[i] = Rmax.dd[i]; Rmaxcount[i]++; } 1657 | else { Rt.dd[i] = (1.0 - Rbeta.dd[i])*Rt.dd[i] + Rbeta.dd[i] * rr; Rmaxcount[i] = 0; } 1658 | Rbeta.dd[i] = Rbeta.dd[i] / (Rbeta.dd[i] + Rb.dd[i]); // beta = beta / (beta+b) 1659 | int adptOK = (Rmaxcount[i] == 0 || Rmaxcount[i] > Rmaxcount0[i]) ? 1 : 0; 1660 | return adptOK; 1661 | } 1662 | 1663 | void CKalman::RPkFading(int i) 1664 | { 1665 | Zfd.dd[i] = Zfd.dd[i] * (1 - zfdafa) + Zk.dd[i] * zfdafa; 1666 | if (Zfd.dd[i] > Zfd0.dd[i] || Zfd.dd[i] < -Zfd0.dd[i]) 1667 | DVMDVafa(Fading.GetRow(i), Pk, 1); 1668 | } 1669 | 1670 | void CKalman::XPConstrain(void) 1671 | { 1672 | int i = 0, nq1 = nq + 1; 1673 | for (double *px = Xk.dd, *pxmax = Xmax.dd, *p = Pk.dd, *pmin = Pmin.dd, *pminEnd = &Pmin.dd[nq], *pmax = Pmax.dd, *pset = Pset.dd; 1674 | pmin < pminEnd; px++, pxmax++, p += nq1, pmin++, pmax++, pset++) 1675 | { 1676 | if (*px > *pxmax) // Xk constrain 1677 | { 1678 | *px = *pxmax; 1679 | } 1680 | else if (*px < -*pxmax) 1681 | { 1682 | *px = -*pxmax; 1683 | } 1684 | if (*p < *pmin) // Pk constrain 1685 | { 1686 | *p = *pmin; 1687 | } 1688 | else if (*p > *pmax) 1689 | { 1690 | double sqf = sqrt(*pmax / (*p))*0.9; 1691 | for (double *prow = &Pk.dd[i*Pk.clm], *prowEnd = prow + nq, *pclm = &Pk.dd[i]; prow < prowEnd; prow++, pclm += nq) 1692 | { 1693 | *prow *= sqf; 1694 | *pclm = *prow; 1695 | } 1696 | Pk.dd[i*Pk.clm + i] *= sqf; //20200303 1697 | break; 1698 | } 1699 | /*if (*pset > 0.0) // Pk set 1700 | { 1701 | if (*p < *pset) 1702 | { 1703 | *p = *pset; 1704 | } 1705 | else if (*p > *pset) 1706 | { 1707 | double sqf = sqrt(*pset / (*p)); 1708 | for (double *prow = &Pk.dd[i*Pk.clm], *prowEnd = prow + nq, *pclm = &Pk.dd[i]; prow < prowEnd; prow++, pclm += nq) 1709 | { 1710 | *prow *= sqf; 1711 | *pclm *= sqf; 1712 | } 1713 | } 1714 | *pset = -1.0; 1715 | }*/ 1716 | i++; 1717 | } 1718 | } 1719 | 1720 | void CKalman::Feedback(double fbts) 1721 | { 1722 | double *pTau = FBTau.dd, *pTotal = FBTotal.dd, *pMax = FBMax.dd, *pOne = FBOne.dd, *pXk = FBXk.dd, *p = Xk.dd; 1723 | for (int i = 0; i < nq; i++, pTau++, pTotal++, pMax++, pOne++, pXk++, p++) 1724 | { 1725 | if (*pTau < INF / 2) 1726 | { 1727 | double afa = fbts < *pTau ? fbts / (*pTau) : 1.0; 1728 | *pXk = *p*afa; 1729 | if (*pXk > *pOne) *pXk = *pOne; else if (*pXk < -*pOne) *pXk = -*pOne; 1730 | if (*pMax < INF / 2) 1731 | { 1732 | if (*pTotal + *pXk > *pMax) *pXk = *pMax - *pTotal; 1733 | else if (*pTotal + *pXk < -*pMax) *pXk = -*pMax - *pTotal; 1734 | } 1735 | *p -= *pXk; 1736 | *pTotal += *pXk; 1737 | } 1738 | else 1739 | { 1740 | *pXk = 0.0; 1741 | } 1742 | } 1743 | } 1744 | 1745 | void CKalman::RtFading(int i, double fdts) 1746 | { 1747 | double Taui = RtTau.dd[i], Rti = Rt.dd[i], Rmaxi = Rmax.dd[i]; 1748 | if (measlost.dd[i] > 3.0 && Taui < INF / 2 && Rti < Rmaxi) 1749 | { 1750 | double afa = fdts < Taui ? fdts / Taui : 1.0; 1751 | Rti += 2 * sqrt(Rmaxi*Rti)*afa; 1752 | Rt.dd[i] = Rti; 1753 | } 1754 | } 1755 | 1756 | void fusion(double *x1, double *p1, const double *x2, const double *p2, int n, double *xf, double *pf) 1757 | { 1758 | if (xf == NULL) { xf = x1, pf = p1; } 1759 | double *x10 = nullptr; 1760 | double *xf0 = nullptr; 1761 | CVect3 att1; 1762 | if (n < 100) { // n<100 for att(1:3), n>100 for no_att(1:3) 1763 | x10 = x1, xf0 = xf; 1764 | att1 = *(CVect3*)x1; 1765 | *(CVect3*)x2 = qq2phi(a2qua(*(CVect3*)x2), a2qua(att1)); 1766 | *(CVect3*)x1 = O31; 1767 | } 1768 | int j; 1769 | for (j = (n > 100) ? 100 : 0; j < n; j++, x1++, p1++, x2++, p2++, xf++, pf++) 1770 | { 1771 | double p1p2 = *p1 + *p2; 1772 | *xf = (*p1**x2 + *p2**x1) / p1p2; 1773 | *pf = *p1**p2 / p1p2; 1774 | } 1775 | if (j < 100) { 1776 | *(CVect3*)xf0 = q2att(a2qua(att1) + *(CVect3*)xf0); 1777 | if (xf0 != x10) *(CVect3*)x10 = att1; 1778 | } 1779 | } 1780 | 1781 | /*************************** class CSINSKF *********************************/ 1782 | CSINSKF::CSINSKF(int nq0, int nr0) :CKalman(nq0, nr0) 1783 | { 1784 | sins = CSINS(qI, O31, O31); 1785 | this->SetFt(nq); 1786 | // an example for SINS/GPS vn&pos measurement 1787 | Hk(0, 3) = Hk(1, 4) = Hk(2, 5) = 1.0; 1788 | Hk(3, 6) = Hk(4, 7) = Hk(5, 8) = 1.0; 1789 | } 1790 | 1791 | void CSINSKF::Init(const CSINS &sins0, int grade) 1792 | { 1793 | sins = sins0; kftk = sins.tk; 1794 | if (grade == -1) return; // NULL 1795 | sins.Rwfa.set( 1796 | CVect(9, 100 * glv.dps, 100 * glv.dps, 100 * glv.dps, 1 * glv.g0, 1 * glv.g0, 1 * glv.g0, 1 * glv.g0, 1 * glv.g0, 1 * glv.g0), 1797 | CVect(9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0), 1798 | CVect(9, 100 * glv.dps, 100 * glv.dps, 100 * glv.dps, 1 * glv.g0, 1 * glv.g0, 1 * glv.g0, 1 * glv.g0, 1 * glv.g0, 1 * glv.g0), 1799 | CVect(9, 0.01*glv.dps, 0.01*glv.dps, 0.01*glv.dps, 0.1*glv.mg, 0.1*glv.mg, 0.1*glv.mg, 0.1*glv.mg, 0.1*glv.mg, 0.1*glv.mg) 1800 | ); 1801 | // a example for 15-state(phi,dvn,dpos,eb,db) KF setting 1802 | if (grade == 0) // inertial-grade 1803 | { 1804 | Pmax.Set2(10.0*glv.deg, 10.0*glv.deg, 30.0*glv.deg, 50.0, 50.0, 50.0, 1.0e4 / glv.Re, 1.0e4 / glv.Re, 1.0e4, 1805 | 10.0*glv.dph, 10.0*glv.dph, 10.0*glv.dph, 10.0*glv.mg, 10.0*glv.mg, 10.0*glv.mg); 1806 | Pmin.Set2(0.01*glv.min, 0.01*glv.min, 0.1*glv.min, 0.01, 0.01, 0.1, 1.0 / glv.Re, 1.0 / glv.Re, 0.1, 1807 | 0.001*glv.dph, 0.001*glv.dph, 0.001*glv.dph, 1.0*glv.ug, 1.0*glv.ug, 5.0*glv.ug); 1808 | Pk.SetDiag2(1.0*glv.deg, 1.0*glv.deg, 10.0*glv.deg, 1.0, 1.0, 1.0, 100.0 / glv.Re, 100.0 / glv.Re, 100.0, 1809 | 0.1*glv.dph, 0.1*glv.dph, 0.1*glv.dph, 1.0*glv.mg, 1.0*glv.mg, 1.0*glv.mg); 1810 | Qt.Set2(0.001*glv.dpsh, 0.001*glv.dpsh, 0.001*glv.dpsh, 1.0*glv.ugpsHz, 1.0*glv.ugpsHz, 1.0*glv.ugpsHz, 0.0, 0.0, 0.0, 1811 | 0.0*glv.dphpsh, 0.0*glv.dphpsh, 0.0*glv.dphpsh, 0.0*glv.ugpsh, 0.0*glv.ugpsh, 0.0*glv.ugpsh); 1812 | Xmax.Set(INF, INF, INF, INF, INF, INF, INF, INF, INF, 0.1*glv.dps, 0.1*glv.dps, 0.1*glv.dps, 200.0*glv.ug, 200.0*glv.ug, 200.0*glv.ug); 1813 | Rt.Set2(0.2, 0.2, 0.6, 10.0 / glv.Re, 10.0 / glv.Re, 30.0); 1814 | Rmax = Rt * 10000; Rmin = Rt * 0.01; Rb = 0.9; 1815 | FBTau.Set(1.0, 1.0, 10.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0); 1816 | } 1817 | else if (grade == 1) // MEMS-grade 1818 | { 1819 | Pmax.Set2(50.0*glv.deg, 50.0*glv.deg, 100.0*glv.deg, 500.0, 500.0, 500.0, 1.0e6 / glv.Re, 1.0e6 / glv.Re, 1.0e6, 1820 | 5000.0*glv.dph, 5000.0*glv.dph, 5000.0*glv.dph, 50.0*glv.mg, 50.0*glv.mg, 50.0*glv.mg); 1821 | Pmin.Set2(0.5*glv.min, 0.5*glv.min, 3.0*glv.min, 0.01, 0.01, 0.1, 1.0 / glv.Re, 1.0 / glv.Re, 0.1, 1822 | 1.0*glv.dph, 1.0*glv.dph, 1.0*glv.dph, 50.0*glv.ug, 50.0*glv.ug, 50.0*glv.ug); 1823 | Pk.SetDiag2(10.0*glv.deg, 10.0*glv.deg, 100.0*glv.deg, 10.0, 10.0, 10.0, 100.0 / glv.Re, 100.0 / glv.Re, 100.0, 1824 | 100.0*glv.dph, 101.0*glv.dph, 102.0*glv.dph, 1.0*glv.mg, 1.01*glv.mg, 10.0*glv.mg); 1825 | Qt.Set2(1.0*glv.dpsh, 1.0*glv.dpsh, 1.0*glv.dpsh, 10.0*glv.ugpsHz, 10.0*glv.ugpsHz, 10.0*glv.ugpsHz, 0.0, 0.0, 0.0, 1826 | 0.0*glv.dphpsh, 0.0*glv.dphpsh, 0.0*glv.dphpsh, 0.0*glv.ugpsh, 0.0*glv.ugpsh, 0.0*glv.ugpsh); 1827 | Xmax.Set(INF, INF, INF, INF, INF, INF, INF, INF, INF, 1.0*glv.dps, 1.0*glv.dps, 1.0*glv.dps, 50.0*glv.mg, 50.0*glv.mg, 50.0*glv.mg); 1828 | Rt.Set2(0.2, 0.2, 0.6, 10.0 / glv.Re, 10.0 / glv.Re, 30.0); 1829 | Rmax = Rt * 10000; Rmin = Rt * 0.01; Rb = 0.9; 1830 | FBTau.Set(1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0); 1831 | } 1832 | Zfd0.Set(1.0, 15.0, 1.0, 100.0 / RE, 100.0 / RE, 100.0, INF); zfdafa = 0.1; 1833 | Fading(0, 1) = 1.01; Fading(0, 3) = 1.01; Fading(0, 7) = 1.01; 1834 | Fading(1, 0) = 1.01; Fading(1, 4) = 1.01; Fading(1, 6) = 1.01; 1835 | Fading(2, 5) = 1.01; Fading(2, 8) = 1.01; Fading(2, 14) = 1.01; 1836 | Fading(3, 0) = 1.01; Fading(3, 4) = 1.01; Fading(3, 6) = 1.01; 1837 | Fading(4, 1) = 1.01; Fading(4, 3) = 1.01; Fading(4, 7) = 1.01; 1838 | Fading(5, 5) = 1.01; Fading(5, 8) = 1.01; Fading(5, 14) = 1.01; 1839 | Zfd0 = INF; 1840 | } 1841 | 1842 | void CSINSKF::SetFt(int nnq) 1843 | { 1844 | sins.etm(); 1845 | // Ft = [ Maa Mav Map -Cnb O33 1846 | // Mva Mvv Mvp O33 Cnb 1847 | // O33 Mpv Mpp O33 O33 1848 | // zeros(6,9) diag(-1./[ins.tauG;ins.tauA]) ]; 1849 | Ft.SetMat3(0, 0, sins.Maa), Ft.SetMat3(0, 3, sins.Mav), Ft.SetMat3(0, 6, sins.Map), Ft.SetMat3(0, 9, -sins.Cnb); 1850 | Ft.SetMat3(3, 0, sins.Mva), Ft.SetMat3(3, 3, sins.Mvv), Ft.SetMat3(3, 6, sins.Mvp), Ft.SetMat3(3, 12, sins.Cnb); 1851 | NULL, Ft.SetMat3(6, 3, sins.Mpv), Ft.SetMat3(6, 6, sins.Mpp); 1852 | Ft.SetDiagVect3(9, 9, sins._betaGyro); 1853 | Ft.SetDiagVect3(12, 12, sins._betaAcc); 1854 | if (nnq >= 34) { 1855 | CMat3 Cwx = -sins.wib.i*sins.Cnb, Cwy = -sins.wib.j*sins.Cnb, Cwz = -sins.wib.k*sins.Cnb, 1856 | Cfx = sins.fb.i *sins.Cnb, Cfy = sins.fb.j *sins.Cnb, Cfz = sins.fb.k *sins.Cnb; 1857 | Cfz.e00 = Cfy.e01, Cfz.e01 = Cfy.e02; 1858 | Cfz.e10 = Cfy.e11, Cfz.e11 = Cfy.e12; 1859 | Cfz.e20 = Cfy.e21, Cfz.e21 = Cfy.e22; 1860 | Ft.SetMat3(0, 19, Cwx); Ft.SetMat3(0, 22, Cwy); Ft.SetMat3(0, 25, Cwz); // dKG 1861 | Ft.SetMat3(3, 28, Cfx); Ft.SetMat3(3, 31, Cfz); // dKA(xx,yx,zx, yy,zy, zz) 1862 | } 1863 | } 1864 | 1865 | void CSINSKF::SetHk(int nnq) 1866 | { 1867 | // ins.CW = ins.Cnb*askew(ins.web); ins.MpvCnb = ins.Mpv*ins.Cnb; 1868 | // Hk(1:3,16:19) = [-ins.CW, -ins.an]; % lever, dt 1869 | // Hk(4:6,16:19) = [-ins.MpvCnb, -ins.Mpvvn]; 1870 | if (nnq >= 18) { 1871 | CMat3 CW = sins.Cnb*askew(sins.web), MC = sins.Mpv*sins.Cnb; 1872 | Hk.SetMat3(0, 15, -CW); 1873 | Hk.SetMat3(3, 15, -MC); 1874 | } 1875 | if (nnq >= 19) { 1876 | CVect3 MV = sins.Mpv*sins.vn; 1877 | Hk.SetClmVect3(0, 18, -sins.an); 1878 | Hk.SetClmVect3(3, 18, -MV); 1879 | } 1880 | } 1881 | 1882 | int CSINSKF::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts) 1883 | { 1884 | sins.Update(pwm, pvm, nSamples, ts); 1885 | TimeUpdate(sins.nts); kftk = sins.tk; 1886 | return MeasUpdate(); 1887 | } 1888 | 1889 | void CSINSKF::Feedback(double fbts) 1890 | { 1891 | CKalman::Feedback(fbts); 1892 | sins.qnb -= *(CVect3*)&FBXk.dd[0]; sins.vn -= *(CVect3*)&FBXk.dd[3]; sins.pos -= *(CVect3*)&FBXk.dd[6]; 1893 | sins.eb += *(CVect3*)&FBXk.dd[9]; sins.db += *(CVect3*)&FBXk.dd[12]; 1894 | } 1895 | 1896 | void CSINSKF::QtMarkovGA(const CVect3 &tauG, const CVect3 &sRG, const CVect3 &tauA = I31, const CVect3 &sRA = O31) 1897 | { 1898 | sins.SetTauGA(tauG, tauA); 1899 | *(CVect3*)&Qt.dd[9] = MKQt(sRG, sins.tauGyro); 1900 | *(CVect3*)&Qt.dd[12] = MKQt(sRA, sins.tauAcc); 1901 | } 1902 | 1903 | void CSINSKF::SetYaw(double yaw) 1904 | { 1905 | CQuat qnn = a2qua(0, 0, diffYaw(yaw, sins.att.k)); 1906 | sins.qnb = qnn * sins.qnb; sins.Cnb = q2mat(sins.qnb); sins.Cbn = ~sins.Cnb; 1907 | sins.vn = qnn * sins.vn; 1908 | *(CVect3*)&Xk.dd[0] = qnn * *(CVect3*)&Xk.dd[0]; 1909 | *(CVect3*)&Xk.dd[3] = qnn * *(CVect3*)&Xk.dd[3]; 1910 | } 1911 | 1912 | void CSINSKF::SecretAttitude(void) 1913 | { 1914 | #define SA_PCH (12*DEG) 1915 | #define SA_RLL (34*DEG) 1916 | #define SA_YAW (56*DEG) 1917 | #define SA_THR (0.1*DEG) 1918 | if (sins.att.iSA_PCH - SA_THR && 1919 | sins.att.jSA_RLL - SA_THR && 1920 | sins.att.kSA_YAW - SA_THR) 1921 | { 1922 | sins.att.k = SA_YAW; 1923 | sins.qnb.SetYaw(sins.att.k); 1924 | } 1925 | #undef SA_PCH 1926 | #undef SA_RLL 1927 | #undef SA_YAW 1928 | #undef SA_THR 1929 | } 1930 | 1931 | /*************************** class CSINSTDKF *********************************/ 1932 | CSINSTDKF::CSINSTDKF(int nq0, int nr0) :CSINSKF(nq0, nr0) 1933 | { 1934 | Fk = Pk1 = CMat(nq, nq, 0.0); 1935 | Pxz = Qk = Kk = tmeas = CVect(nr, 0.0); 1936 | meantdts = 1.0; tdts = 0.0; 1937 | maxStep = 2 * (nq + nr) + 3; 1938 | TDReset(); 1939 | } 1940 | 1941 | void CSINSTDKF::TDReset(void) 1942 | { 1943 | iter = -2; 1944 | ifn = 0; meanfn = O31; 1945 | SetMeasFlag(0); 1946 | } 1947 | 1948 | int CSINSTDKF::TDUpdate(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts, int nStep) 1949 | { 1950 | // if(sins.tk>99) 1951 | // int debugi = 1; 1952 | 1953 | sins.Update(pwm, pvm, nSamples, ts); 1954 | Feedback(sins.nts); 1955 | for (int j = 0; j < nr; j++) measlost.dd[j] += sins.nts; 1956 | 1957 | measRes = 0; 1958 | 1959 | if (nStep <= 0 || nStep >= maxStep) { nStep = maxStep; } 1960 | tdStep = nStep; 1961 | 1962 | tdts += sins.nts; kftk = sins.tk; 1963 | meanfn = meanfn + sins.fn; ifn++; 1964 | for (int i = 0; i < nStep; i++) 1965 | { 1966 | if (iter == -2) // -2: set measurements 1967 | { 1968 | if (ifn == 0) break; 1969 | CVect3 vtmp = meanfn * (1.0 / ifn); meanfn = O31; ifn = 0; 1970 | sins.fn = vtmp; SetFt(nq); sins.fn = vtmp; 1971 | SetMeas(); SetHk(nq); 1972 | } 1973 | else if (iter == -1) // -1: discrete 1974 | { 1975 | Fk = ++(Ft*tdts); // Fk = I+Ft*ts 1976 | Qk = Qt * tdts; 1977 | Xk = Fk * Xk; 1978 | // RtFading(tdts); 1979 | meantdts = tdts; tdts = 0.0; 1980 | } 1981 | else if (iter < nq) // 0 -> (nq-1): Fk*Pk 1982 | { 1983 | int row = iter; 1984 | RowMul(Pk1, Fk, Pk, row); 1985 | } 1986 | else if (iter < 2 * nq) // nq -> (2*nq-1): Fk*Pk*Fk+Qk 1987 | { 1988 | int row = iter - nq; 1989 | RowMulT(Pk, Pk1, Fk, row); 1990 | Pk.dd[nq*row + row] += Qk.dd[row]; 1991 | // if(row==nq-1) { Pk += Qk; } 1992 | } 1993 | else if (iter < 2 * (nq + nr)) // (2*nq) -> (2*(nq+nr)-1): sequential measurement updating 1994 | { 1995 | if (measstop > 0) measflag = 0; 1996 | int row = (iter - 2 * Ft.row) / 2; 1997 | int flag = measflag & (0x01 << row); 1998 | if (flag) 1999 | { 2000 | // if((iter-2*Ft.row)%2==0) 2001 | if (iter % 2 == 0) 2002 | { 2003 | Hi = Hk.GetRow(row); 2004 | Pxz = Pk * (~Hi); 2005 | Pz0 = (Hi*Pxz)(0, 0); 2006 | innovation = Zk(row) - (Hi*Xk)(0, 0); 2007 | adptOKi = 1; 2008 | if (Rb.dd[row] > EPS) 2009 | adptOKi = RAdaptive(row, innovation, Pz0); 2010 | double Pzz = Pz0 + Rt(row) / rts(row); 2011 | Kk = Pxz * (1.0 / Pzz); 2012 | } 2013 | else 2014 | { 2015 | measflag ^= flag; 2016 | if (adptOKi) 2017 | { 2018 | measRes |= flag; 2019 | Xk += Kk * innovation; 2020 | Pk -= Kk * (~Pxz); 2021 | measlost.dd[row] = 0.0; 2022 | } 2023 | if (Zfd0.dd[row] < INF / 2) 2024 | { 2025 | RPkFading(row); 2026 | } 2027 | } 2028 | } 2029 | else 2030 | { 2031 | nStep++; 2032 | } 2033 | if (iter % 2 == 0) 2034 | RtFading(row, meantdts); 2035 | } 2036 | else if (iter == 2 * (nq + nr)) // 2*(nq+nr): Xk,Pk constrain & symmetry 2037 | { 2038 | XPConstrain(); 2039 | symmetry(Pk); 2040 | } 2041 | else if (iter >= 2 * (nq + nr) + 1) // 2*(nq+nr)+1: Miscellanous 2042 | { 2043 | Miscellanous(); 2044 | iter = -3; 2045 | } 2046 | iter++; 2047 | } 2048 | SecretAttitude(); 2049 | if (measstop > -1000000) measstop--; 2050 | 2051 | measflaglog |= measRes; 2052 | return measRes; 2053 | } 2054 | 2055 | /*************************** class CSINSGPS *********************************/ 2056 | CSINSGPSOD::CSINSGPSOD(void) :CSINSTDKF(21, 10) 2057 | { 2058 | tODInt = 0.0; Kod = 1.0; 2059 | Cbo = I33; MpkD = O33; 2060 | Hk.SetMat3(6, 6, I33); Hk.SetMat3(6, 18, -I33); 2061 | Hk(9, 2) = 1.0; 2062 | measGPSvnValid = measGPSposValid = measODValid = measMAGyawValid = 0; 2063 | } 2064 | 2065 | #ifndef CSINSGPSOD_INIT_BY_USER 2066 | // may be copied & implemented by user 2067 | void CSINSGPSOD::Init(const CSINS &sins0, int grade) 2068 | { 2069 | CSINSKF::Init(sins0); 2070 | posDR = sins0.pos; 2071 | Pmax.Set2(10.0*glv.deg, 10.0*glv.deg, 30.0*glv.deg, 50.0, 50.0, 50.0, 1.0e4 / glv.Re, 1.0e4 / glv.Re, 1.0e4, 2072 | 10.0*glv.dph, 10.0*glv.dph, 10.0*glv.dph, 10.0*glv.mg, 10.0*glv.mg, 10.0*glv.mg, 1 * glv.deg, 0.01, 1 * glv.deg, 1.0e4 / glv.Re, 1.0e4 / glv.Re, 1.0e4); 2073 | Pmin.Set2(0.01*glv.min, 0.01*glv.min, 0.1*glv.min, 0.01, 0.01, 0.1, 1.0 / glv.Re, 1.0 / glv.Re, 0.1, 2074 | 0.001*glv.dph, 0.001*glv.dph, 0.001*glv.dph, 5.0*glv.ug, 5.0*glv.ug, 15.0*glv.ug, 1 * glv.min, 0.0001, 0.2*glv.min, 1.0 / glv.Re, 1.0 / glv.Re, 1.0); 2075 | Pk.SetDiag2(1.0*glv.deg, 1.0*glv.deg, 1.0*glv.deg, 1.0, 1.0, 1.0, 100.0 / glv.Re, 100.0 / glv.Re, 100.0, 2076 | 0.01*glv.dph, 0.01*glv.dph, 0.01*glv.dph, .10*glv.mg, .10*glv.mg, .10*glv.mg, 10.0*glv.min, 0.05, 15 * glv.min, 1.0e1 / glv.Re, 1.0e1 / glv.Re, 1.0e1); 2077 | Qt.Set2(0.001*glv.dpsh, 0.001*glv.dpsh, 0.001*glv.dpsh, 1.0*glv.ugpsHz, 1.0*glv.ugpsHz, 1.0*glv.ugpsHz, 0.0, 0.0, 0.0, 2078 | 0.0*glv.dphpsh, 0.0*glv.dphpsh, 0.0*glv.dphpsh, 0.0*glv.ugpsh, 0.0*glv.ugpsh, 0.0*glv.ugpsh, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); 2079 | Xmax.Set(INF, INF, INF, INF, INF, INF, INF, INF, INF, 0.1*glv.dph, 0.1*glv.dph, 0.1*glv.dph, 200.0*glv.ug, 200.0*glv.ug, 200.0*glv.ug, 1 * glv.deg, 0.05, 1 * glv.deg, INF, INF, INF); 2080 | Rt.Set2(0.2, 0.2, 0.6, 10.0 / glv.Re, 10.0 / glv.Re, 30.0, 10.1 / RE, 10.1 / RE, 10.1); 2081 | Rmax = Rt * 100; Rmin = Rt * 0.01; Rb = 0.9; 2082 | FBTau.Set(1.0, 1.0, 10.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0); 2083 | // FBTau.Set(1.0, 1.0, 10.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, INF, INF, INF); 2084 | } 2085 | #else 2086 | #pragma message(" CSINSGPSOD_INIT_BY_USER") 2087 | #endif 2088 | 2089 | void CSINSGPSOD::SetFt(int nnq) 2090 | { 2091 | CSINSKF::SetFt(15); 2092 | CMat3 MvkD = norm(sins.vn)*CMat3(-sins.Cnb.e02, sins.Cnb.e01, sins.Cnb.e00, 2093 | -sins.Cnb.e12, sins.Cnb.e11, sins.Cnb.e10, -sins.Cnb.e22, sins.Cnb.e21, sins.Cnb.e20); 2094 | Ft.SetMat3(18, 0, sins.Mpv*askew(sins.vn)); 2095 | Ft.SetMat3(18, 15, sins.Mpv*MvkD); 2096 | Ft.SetMat3(18, 18, sins.Mpp); 2097 | } 2098 | 2099 | void CSINSGPSOD::SetMeas(void) 2100 | { 2101 | if (measGPSvnValid) { SetMeasFlag(000007); } // SINS/GPS_VEL 2102 | if (measGPSposValid) { SetMeasFlag(000070); } // SINS/GPS_POS 2103 | if (measODValid) { SetMeasFlag(000700); } // SINS/DR 2104 | if (measMAGyawValid) { SetMeasFlag(001000); } // SINS/MAG_YAW 2105 | measGPSvnValid = measGPSposValid = measODValid = measMAGyawValid = 0; 2106 | } 2107 | 2108 | void CSINSGPSOD::Feedback(double fbts) 2109 | { 2110 | CSINSKF::Feedback(fbts); 2111 | Cbo = Cbo * a2mat(CVect3(FBXk.dd[15], 0.0, FBXk.dd[17])); 2112 | Kod *= 1 - FBXk.dd[16]; 2113 | posDR -= *(CVect3*)(&FBXk.dd[18]); 2114 | } 2115 | 2116 | void CSINSGPSOD::SetMeasGPS(const CVect3 &pgps, const CVect3 &vgps) 2117 | { 2118 | if (!IsZero(pgps, EPS)) 2119 | { 2120 | *(CVect3*)&Zk.dd[3] = sins.pos - pgps; 2121 | measGPSposValid = 1; 2122 | } 2123 | if (!IsZero(vgps, EPS)) 2124 | { 2125 | *(CVect3*)&Zk.dd[0] = sins.vn - vgps; 2126 | measGPSvnValid = 1; 2127 | } 2128 | } 2129 | 2130 | void CSINSGPSOD::SetMeasOD(double dSod, double ts) 2131 | { 2132 | CVect3 dSn = sins.Cnb*(Cbo*CVect3(0, dSod*Kod, 0)); 2133 | posDR = posDR + sins.eth.vn2dpos(dSn, 1.0); 2134 | tODInt += ts; 2135 | if (fabs(sins.wnb.k) < .5*glv.dps) 2136 | { 2137 | if (tODInt > 1.10) 2138 | { 2139 | *(CVect3*)&Zk.dd[6] = sins.pos - posDR; 2140 | tODInt = 0.0; 2141 | measODValid = 1; 2142 | } 2143 | } 2144 | else 2145 | { 2146 | tODInt = 0.0; 2147 | } 2148 | } 2149 | 2150 | void CSINSGPSOD::SetMeasYaw(double ymag) 2151 | { 2152 | if (ymag > EPS || ymag < -EPS) // !IsZero(yawGPS) 2153 | if (sins.att.i<30 * DEG && sins.att.i>-30 * DEG) 2154 | { 2155 | *(CVect3*)&Zk.dd[9] = -diffYaw(sins.att.k, ymag); 2156 | measMAGyawValid = 1; 2157 | } 2158 | } 2159 | 2160 | int CSINSGPSOD::Update(const CVect3 *pwm, const CVect3 *pvm, int nn, double ts) 2161 | { 2162 | int res = TDUpdate(pwm, pvm, nn, ts, 5); 2163 | return res; 2164 | } 2165 | 2166 | /*************************** class CEarth *********************************/ 2167 | CEarth::CEarth(double a0, double f0, double g0) 2168 | { 2169 | a = a0; f = f0; wie = glv.wie; 2170 | b = (1 - f)*a; 2171 | e = sqrt(a*a - b * b) / a; e2 = e * e; 2172 | gn = O31; pgn = 0; 2173 | Update(O31); 2174 | } 2175 | 2176 | void CEarth::Update(const CVect3 &pos, const CVect3 &vn) 2177 | { 2178 | #ifdef PSINS_LOW_GRADE_MEMS 2179 | this->pos = pos; this->vn = vn; 2180 | sl = sin(pos.i), cl = cos(pos.i), tl = sl / cl; 2181 | double sq = 1 - e2 * sl*sl, sq2 = sqrt(sq); 2182 | RMh = a * (1 - e2) / sq / sq2 + pos.k; f_RMh = 1.0 / RMh; 2183 | RNh = a / sq2 + pos.k; clRNh = cl * RNh; f_RNh = 1.0 / RNh; f_clRNh = 1.0 / clRNh; 2184 | // wnie.i = 0.0, wnie.j = wie*cl, wnie.k = wie*sl; 2185 | // wnen.i = -vn.j*f_RMh, wnen.j = vn.i*f_RNh, wnen.k = wnen.j*tl; 2186 | wnin = wnie = wnen = O31; 2187 | sl2 = sl * sl; 2188 | gn.k = -(glv.g0*(1 + 5.27094e-3*sl2) - 3.086e-6*pos.k); 2189 | gcc = pgn ? *pgn : gn; 2190 | #else 2191 | this->pos = pos; this->vn = vn; 2192 | sl = sin(pos.i), cl = cos(pos.i), tl = sl / cl; 2193 | double sq = 1 - e2 * sl*sl, sq2 = sqrt(sq); 2194 | RMh = a * (1 - e2) / sq / sq2 + pos.k; f_RMh = 1.0 / RMh; 2195 | RNh = a / sq2 + pos.k; clRNh = cl * RNh; f_RNh = 1.0 / RNh; f_clRNh = 1.0 / clRNh; 2196 | wnie.i = 0.0, wnie.j = wie * cl, wnie.k = wie * sl; 2197 | wnen.i = -vn.j*f_RMh, wnen.j = vn.i*f_RNh, wnen.k = wnen.j*tl; 2198 | wnin = wnie + wnen; 2199 | sl2 = sl * sl, sl4 = sl2 * sl2; 2200 | gn.k = -(glv.g0*(1 + 5.27094e-3*sl2 + 2.32718e-5*sl4) - 3.086e-6*pos.k); 2201 | gcc = pgn ? *pgn : gn; 2202 | gcc -= (wnie + wnin)*vn; 2203 | #endif 2204 | } 2205 | 2206 | CVect3 CEarth::vn2dpos(const CVect3 &vn, double ts) const 2207 | { 2208 | return CVect3(vn.j*f_RMh, vn.i*f_clRNh, vn.k)*ts; 2209 | } 2210 | 2211 | /*************************** class CIMU *********************************/ 2212 | CIMU::CIMU(void) 2213 | { 2214 | nSamples = 1; 2215 | preFirst = onePlusPre = true; 2216 | phim = dvbm = wm_1 = vm_1 = O31; 2217 | } 2218 | 2219 | void CIMU::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples) 2220 | { 2221 | static double conefactors[5][4] = { // coning coefficients 2222 | {2. / 3}, // 2 2223 | {9. / 20, 27. / 20}, // 3 2224 | {54. / 105, 92. / 105, 214. / 105}, // 4 2225 | {250. / 504, 525. / 504, 650. / 504, 1375. / 504} // 5 2226 | }; 2227 | int i; 2228 | double *pcf = conefactors[nSamples - 2]; 2229 | CVect3 cm(0.0), sm(0.0), wmm(0.0), vmm(0.0); 2230 | 2231 | psinsassert(nSamples > 0 && nSamples < 6); 2232 | this->nSamples = nSamples; 2233 | if (nSamples == 1 && onePlusPre) // one-plus-previous sample 2234 | { 2235 | if (preFirst) { wm_1 = pwm[0]; vm_1 = pvm[0]; preFirst = false; } 2236 | cm = 1.0 / 12 * wm_1; 2237 | sm = 1.0 / 12 * vm_1; 2238 | } 2239 | for (i = 0; i < nSamples - 1; i++) 2240 | { 2241 | cm += pcf[i] * pwm[i]; 2242 | sm += pcf[i] * pvm[i]; 2243 | wmm += pwm[i]; 2244 | vmm += pvm[i]; 2245 | } 2246 | wm_1 = pwm[i]; vm_1 = pvm[i]; 2247 | wmm += pwm[i]; 2248 | vmm += pvm[i]; 2249 | phim = wmm + cm * pwm[i]; 2250 | dvbm = vmm + 1.0 / 2 * wmm*vmm + (cm*pvm[i] + sm * pwm[i]); 2251 | } 2252 | 2253 | void IMURFU(CVect3 *pwm, int nSamples, const char *str) 2254 | { 2255 | for (int n = 0; n < nSamples; n++) 2256 | { 2257 | CVect3 tmpwm; 2258 | double *pw = (double*)&pwm[n].i; 2259 | for (int i = 0; i < 3; i++, pw++) 2260 | { 2261 | switch (str[i]) 2262 | { 2263 | case 'R': tmpwm.i = *pw; break; 2264 | case 'L': tmpwm.i = -*pw; break; 2265 | case 'F': tmpwm.j = *pw; break; 2266 | case 'B': tmpwm.j = -*pw; break; 2267 | case 'U': tmpwm.k = *pw; break; 2268 | case 'D': tmpwm.k = -*pw; break; 2269 | } 2270 | } 2271 | pwm[n] = tmpwm; 2272 | } 2273 | } 2274 | 2275 | void IMURFU(CVect3 *pwm, CVect3 *pvm, int nSamples, const char *str) 2276 | { 2277 | IMURFU(pwm, nSamples, str); 2278 | IMURFU(pvm, nSamples, str); 2279 | } 2280 | 2281 | /*************************** class CSINS *********************************/ 2282 | CSINS::CSINS(const CVect3 &att0, const CVect3 &vn0, const CVect3 &pos0, double tk0) 2283 | { 2284 | Init(a2qua(att0), vn0, pos0, tk0); 2285 | } 2286 | 2287 | CSINS::CSINS(const CQuat &qnb0, const CVect3 &vn0, const CVect3 &pos0, double tk0) 2288 | { 2289 | Init(qnb0, vn0, pos0, tk0); 2290 | } 2291 | 2292 | void CSINS::Init(const CQuat &qnb0, const CVect3 &vn0, const CVect3 &pos0, double tk0) 2293 | { 2294 | tk = tk0; ts = nts = 1.0; 2295 | velMax = 400.0; hgtMin = -RE * 0.01, hgtMax = -hgtMin; 2296 | qnb = qnb0; vn = vn0, pos = pos0; 2297 | Kg = Ka = I33; eb = db = Ka2 = O31; 2298 | Maa = Mav = Map = Mva = Mvv = Mvp = Mpv = Mpp = O33; 2299 | SetTauGA(CVect3(INF), CVect3(INF)); 2300 | CVect3 wib(0.0), fb = (~qnb)*CVect3(0, 0, glv.g0); 2301 | lvr = an = O31; 2302 | Rwfa = CRAvar(9); 2303 | isOpenloop = 0; 2304 | Update(&wib, &fb, 1, 1.0); imu.preFirst = 1; 2305 | tk = tk0; ts = nts = 1.0; qnb = qnb0; vn = vn0, pos = pos0; 2306 | etm(); lever(); Extrap(); 2307 | } 2308 | 2309 | void CSINS::SetTauGA(const CVect3 &tauG, const CVect3 &tauA) 2310 | { 2311 | tauGyro = tauG, tauAcc = tauA; 2312 | _betaGyro.i = tauG.i > INF / 2 ? 0.0 : -1.0 / tauG.i; // Gyro&Acc inverse correlation time for AR(1) model 2313 | _betaGyro.j = tauG.j > INF / 2 ? 0.0 : -1.0 / tauG.j; 2314 | _betaGyro.k = tauG.k > INF / 2 ? 0.0 : -1.0 / tauG.k; 2315 | _betaAcc.i = tauA.i > INF / 2 ? 0.0 : -1.0 / tauA.i; 2316 | _betaAcc.j = tauA.j > INF / 2 ? 0.0 : -1.0 / tauA.j; 2317 | _betaAcc.k = tauA.k > INF / 2 ? 0.0 : -1.0 / tauA.k; 2318 | } 2319 | 2320 | void CSINS::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts) 2321 | { 2322 | #ifdef PSINS_LOW_GRADE_MEMS 2323 | #pragma message(" PSINS_LOW_GRADE_MEMS") 2324 | this->ts = ts; nts = nSamples * ts; tk += nts; 2325 | double nts2 = nts / 2; 2326 | imu.Update(pwm, pvm, nSamples); 2327 | imu.phim = Kg * imu.phim - eb * nts; imu.dvbm = Ka * imu.dvbm - db * nts; // IMU calibration 2328 | imu.dvbm.i -= Ka2.i*imu.dvbm.i*imu.dvbm.i / nts; 2329 | imu.dvbm.j -= Ka2.j*imu.dvbm.j*imu.dvbm.j / nts; 2330 | imu.dvbm.k -= Ka2.k*imu.dvbm.k*imu.dvbm.k / nts; 2331 | // CVect3 vn01 = vn+an*nts2, pos01 = pos+eth.vn2dpos(vn01,nts2); 2332 | if (!isOpenloop) eth.Update(pos); 2333 | wib = imu.phim / nts; fb = imu.dvbm / nts; 2334 | web = wib; 2335 | wnb = wib; 2336 | fn = qnb * fb; 2337 | an = fn + eth.gcc; 2338 | CVect3 vn1 = vn + an * nts; 2339 | pos = pos + eth.vn2dpos(vn + vn1, nts2); vn = vn1; 2340 | qnb = qnb * rv2q(imu.phim); 2341 | Cnb = q2mat(qnb); att = m2att(Cnb); Cbn = ~Cnb; vb = Cbn * vn; 2342 | #else 2343 | this->ts = ts; nts = nSamples * ts; tk += nts; 2344 | double nts2 = nts / 2; 2345 | imu.Update(pwm, pvm, nSamples); 2346 | imu.phim = Kg * imu.phim - eb * nts; imu.dvbm = Ka * imu.dvbm - db * nts; // IMU calibration 2347 | CVect3 vn01 = vn + an * nts2, pos01 = pos + eth.vn2dpos(vn01, nts2); 2348 | if (!isOpenloop) eth.Update(pos01, vn01); 2349 | wib = imu.phim / nts; fb = imu.dvbm / nts; 2350 | web = wib - Cbn * eth.wnie; 2351 | // wnb = wib - (~(qnb*rv2q(imu.phim/2)))*eth.wnin; 2352 | wnb = wib - Cbn * eth.wnin; 2353 | fn = qnb * fb; 2354 | an = rv2q(-eth.wnin*nts2)*fn + eth.gcc; 2355 | CVect3 vn1 = vn + an * nts; 2356 | pos = pos + eth.vn2dpos(vn + vn1, nts2); vn = vn1; 2357 | qnb = rv2q(-eth.wnin*nts)*qnb*rv2q(imu.phim); 2358 | Cnb = q2mat(qnb); att = m2att(Cnb); Cbn = ~Cnb; vb = Cbn * vn; 2359 | #endif 2360 | psinsassert(pos.i<85.0*PI / 180 && pos.i>-85.0*PI / 180); 2361 | if (vn.i > velMax) vn.i = velMax; else if (vn.i < -velMax) vn.i = -velMax; 2362 | if (vn.j > velMax) vn.j = velMax; else if (vn.j < -velMax) vn.j = -velMax; 2363 | if (vn.k > velMax) vn.k = velMax; else if (vn.k < -velMax) vn.k = -velMax; 2364 | if (pos.j > PI) pos.j -= _2PI; else if (pos.j < -PI) pos.j += _2PI; 2365 | if (pos.k > hgtMax) pos.k = hgtMax; else if (pos.k < hgtMin) pos.k = hgtMin; 2366 | CVect wfa(9); 2367 | *(CVect3*)&wfa.dd[0] = wib, *(CVect3*)&wfa.dd[3] = fb, *(CVect3*)&wfa.dd[6] = Cbn * an; 2368 | Rwfa.Update(wfa, nts); 2369 | } 2370 | 2371 | void CSINS::Extrap(const CVect3 &wm, const CVect3 &vm, double ts) 2372 | { 2373 | if (ts < 1.0e-6) // reset 2374 | { 2375 | qnbE = qnb, vnE = vn, posE = pos, attE = att; 2376 | } 2377 | else 2378 | { 2379 | vnE = vnE + qnbE * vm + eth.gcc*ts; 2380 | posE = posE + eth.vn2dpos(vnE, ts); 2381 | qnbE = qnbE * rv2q(wm); attE = q2att(qnbE); 2382 | } 2383 | } 2384 | 2385 | void CSINS::Extrap(double extts) 2386 | { 2387 | double k = extts / nts; 2388 | vnE = vn + qnb * imu.dvbm*k + eth.gcc*extts; 2389 | posE = pos + eth.vn2dpos(vn, extts); 2390 | attE = q2att(qnb*rv2q(imu.phim*k)); 2391 | } 2392 | 2393 | void CSINS::lever(const CVect3 &dL) 2394 | { 2395 | if (!IsZero(dL, EPS)) lvr = dL; 2396 | Mpv = CMat3(0, eth.f_RMh, 0, eth.f_clRNh, 0, 0, 0, 0, 1); 2397 | CW = Cnb * askew(web), MpvCnb = Mpv * Cnb; 2398 | vnL = vn + CW * lvr; posL = pos + MpvCnb * lvr; 2399 | } 2400 | 2401 | void CSINS::etm(void) 2402 | { 2403 | #ifdef PSINS_LOW_GRADE_MEMS 2404 | Mva = askew(fn); 2405 | Mpv = CMat3(0, eth.f_RMh, 0, eth.f_clRNh, 0, 0, 0, 0, 1); 2406 | #else 2407 | double tl = eth.tl, secl = 1.0 / eth.cl, secl2 = secl * secl, 2408 | wN = eth.wnie.j, wU = eth.wnie.k, vE = vn.i, vN = vn.j; 2409 | double f_RMh = eth.f_RMh, f_RNh = eth.f_RNh, f_clRNh = eth.f_clRNh, 2410 | f_RMh2 = f_RMh * f_RMh, f_RNh2 = f_RNh * f_RNh; 2411 | CMat3 Avn = askew(vn), 2412 | Mp1(0, 0, 0, -wU, 0, 0, wN, 0, 0), 2413 | Mp2(0, 0, vN*f_RMh2, 0, 0, -vE * f_RNh2, vE*secl2*f_RNh, 0, -vE * tl*f_RNh2); 2414 | Maa = askew(-eth.wnin); 2415 | Mav = CMat3(0, -f_RMh, 0, f_RNh, 0, 0, tl*f_RNh, 0, 0); 2416 | Map = Mp1 + Mp2; 2417 | Mva = askew(fn); 2418 | Mvv = Avn * Mav - askew(eth.wnie + eth.wnin); 2419 | Mvp = Avn * (Mp1 + Map); 2420 | double scl = eth.sl*eth.cl; 2421 | Mvp.e20 = Mvp.e20 - glv.g0*(5.27094e-3 * 2 * scl + 2.32718e-5 * 4 * eth.sl2*scl); Mvp.e22 = Mvp.e22 + 3.086e-6; 2422 | Mpv = CMat3(0, f_RMh, 0, f_clRNh, 0, 0, 0, 0, 1); 2423 | Mpp = CMat3(0, 0, -vN * f_RMh2, vE*tl*f_clRNh, 0, -vE * secl*f_RNh2, 0, 0, 0); 2424 | #endif 2425 | } 2426 | 2427 | CAVPInterp::CAVPInterp(double ts) 2428 | { 2429 | this->ts = ts; 2430 | ipush = 0; 2431 | att = vn = pos = 0.0; 2432 | for (int i = 0; i < AVPINUM; i++) atti[i] = vni[i] = posi[i] = i; 2433 | } 2434 | 2435 | void CAVPInterp::Push(const CVect3 &attk, const CVect3 &vnk, const CVect3 &posk) 2436 | { 2437 | if (++ipush >= AVPINUM) ipush = 0; 2438 | atti[ipush] = attk; vni[ipush] = vnk; posi[ipush] = posk; 2439 | } 2440 | 2441 | int CAVPInterp::Interp(double tpast) 2442 | { 2443 | int res = 1, k, k1, k2; 2444 | if (tpast < -AVPINUM * ts || tpast>0) return (res = 0); 2445 | k = (int)(-tpast / ts); 2446 | if ((k2 = ipush - k) < 0) k2 += AVPINUM; 2447 | if ((k1 = k2 - 1) < 0) k1 += AVPINUM; 2448 | double tleft = -tpast - k * ts; 2449 | if (tleft > 0.99*ts) 2450 | { 2451 | att = atti[k1], vn = vni[k1], pos = posi[k1]; 2452 | } 2453 | else if (tleft < 0.01*ts) 2454 | { 2455 | att = atti[k2], vn = vni[k2], pos = posi[k2]; 2456 | } 2457 | else 2458 | { 2459 | double b = tleft / ts, a = 1 - b; 2460 | att = b * atti[k1] + a * atti[k2], vn = b * vni[k1] + a * vni[k2], pos = b * posi[k1] + a * posi[k2]; 2461 | if (norm(att - atti[k1]) > 10.0*DEG) res = 0; 2462 | else if (fabs(pos.j - posi[k1].j) > 1.0*DEG) res = 0; 2463 | } 2464 | return res; 2465 | } 2466 | 2467 | 2468 | #ifdef PSINS_AHRS_MEMS 2469 | #pragma message(" PSINS_AHRS_MEMS") 2470 | 2471 | /********************* class CMahony AHRS ************************/ 2472 | CMahony::CMahony(double tau, const CQuat &qnb0) 2473 | { 2474 | SetTau(tau); 2475 | qnb = qnb0; 2476 | Cnb = q2mat(qnb); 2477 | exyzInt = O31; ebMax = I31 * glv.dps; 2478 | tk = 0.0; 2479 | } 2480 | 2481 | void CMahony::SetTau(double tau) 2482 | { 2483 | double beta = 2.146 / tau; 2484 | Kp = 2.0f*beta, Ki = beta * beta; 2485 | } 2486 | 2487 | void CMahony::Update(const CVect3 &wm, const CVect3 &vm, double ts, const CVect3 &mag) 2488 | { 2489 | double nm; 2490 | CVect3 acc0, mag0, exyz, bxyz, wxyz; 2491 | 2492 | nm = norm(vm) / ts; // f (in m/s^2) 2493 | acc0 = nm > 0.1 ? vm / (nm*ts) : O31; 2494 | nm = norm(mag); // mag (in Gauss) 2495 | if (nm > 0.1) 2496 | { 2497 | mag0 = mag / nm; 2498 | bxyz = Cnb * mag0; 2499 | bxyz.j = normXY(bxyz); bxyz.i = 0.0; 2500 | wxyz = (~Cnb)*bxyz; 2501 | } 2502 | else 2503 | { 2504 | mag0 = wxyz = O31; 2505 | } 2506 | exyz = *((CVect3*)&Cnb.e20)*acc0 + wxyz * mag0; 2507 | exyzInt += exyz * (Ki*ts); 2508 | qnb *= rv2q(wm - (Kp*exyz + exyzInt)*ts); 2509 | Cnb = q2mat(qnb); 2510 | tk += ts; 2511 | if (exyzInt.i > ebMax.i) exyzInt.i = ebMax.i; else if (exyzInt.i < -ebMax.i) exyzInt.i = -ebMax.i; 2512 | if (exyzInt.j > ebMax.j) exyzInt.j = ebMax.j; else if (exyzInt.j < -ebMax.j) exyzInt.j = -ebMax.j; 2513 | if (exyzInt.k > ebMax.k) exyzInt.k = ebMax.k; else if (exyzInt.k < -ebMax.k) exyzInt.k = -ebMax.k; 2514 | } 2515 | 2516 | void CMahony::Update(const CVect3 &gyro, const CVect3 &acc, const CVect3 &mag, double ts) 2517 | { 2518 | double nm; 2519 | CVect3 acc0, mag0, exyz, bxyz, wxyz; 2520 | 2521 | nm = norm(acc); 2522 | acc0 = nm > 0.1 ? acc / nm : O31; 2523 | nm = norm(mag); 2524 | mag0 = nm > 0.1 ? mag / nm : O31; 2525 | bxyz = Cnb * mag0; 2526 | bxyz.j = normXY(bxyz); bxyz.i = 0.0; 2527 | wxyz = (~Cnb)*bxyz; 2528 | exyz = *((CVect3*)&Cnb.e20)*acc0 + wxyz * mag0; 2529 | exyzInt += exyz * (Ki*ts); 2530 | qnb *= rv2q((gyro*glv.dps - Kp * exyz - exyzInt)*ts); 2531 | Cnb = q2mat(qnb); 2532 | tk += ts; 2533 | } 2534 | 2535 | /********************* class Quat&EKF based AHRS ************************/ 2536 | CQEAHRS::CQEAHRS(double ts) :CKalman(7, 3) 2537 | { 2538 | double sts = sqrt(ts); 2539 | Pmax.Set2(2.0, 2.0, 2.0, 2.0, 1000 * glv.dph, 1000.0*glv.dph, 1000.0*glv.dph); 2540 | Pmin.Set2(0.001, 0.001, 0.001, 0.001, 10.0*glv.dph, 10.0*glv.dph, 10.0*glv.dph); 2541 | Pk.SetDiag2(1.0, 1.0, 1.0, 1.0, 1000.0*glv.dph, 1000.0*glv.dph, 1000.0*glv.dph); 2542 | Qt.Set2(10.0*glv.dpsh, 10.0*glv.dpsh, 10.0*glv.dpsh, 10.0*glv.dphpsh, 10.0*glv.dphpsh, 10.0*glv.dphpsh); 2543 | Rt.Set2(100.0*glv.mg / sts, 100.0*glv.mg / sts, 1.0*glv.deg / sts); 2544 | Xk(0) = 1.0; 2545 | Cnb = q2mat(*(CQuat*)&Xk.dd[0]); 2546 | } 2547 | 2548 | void CQEAHRS::Update(const CVect3 &gyro, const CVect3 &acc, const CVect3 &mag, double ts) 2549 | { 2550 | double q0, q1, q2, q3, wx, wy, wz, fx, fy, fz, mx, my, mz, h11, h12, h21, h22; 2551 | q0 = Xk.dd[0], q1 = Xk.dd[1], q2 = Xk.dd[2], q3 = Xk.dd[3]; 2552 | wx = gyro.i*glv.dps, wy = gyro.j*glv.dps, wz = gyro.k*glv.dps; 2553 | fx = acc.i, fy = acc.j, fz = acc.k; 2554 | mx = mag.i, my = mag.j, mz = mag.k; 2555 | // Ft 2556 | 0, Ft.dd[1] = -wx / 2, Ft.dd[2] = -wy / 2, Ft.dd[3] = -wz / 2, Ft.dd[4] = q1 / 2, Ft.dd[5] = q2 / 2, Ft.dd[6] = q3 / 2; 2557 | Ft.dd[7] = wx / 2, 0, Ft.dd[9] = wz / 2, Ft.dd[10] = -wy / 2, Ft.dd[11] = -q0 / 2, Ft.dd[12] = q3 / 2, Ft.dd[13] = -q2 / 2; 2558 | Ft.dd[14] = wy / 2, Ft.dd[15] = -wz / 2, 0, Ft.dd[17] = wx / 2, Ft.dd[18] = -q3 / 2, Ft.dd[18] = -q0 / 2, Ft.dd[20] = q1 / 2; 2559 | Ft.dd[21] = wz / 2, Ft.dd[22] = wy / 2, Ft.dd[23] = -wx / 2, 0, Ft.dd[25] = q2 / 2, Ft.dd[26] = -q1 / 2, Ft.dd[27] = -q0 / 2; 2560 | // Hk 2561 | h11 = fx * q0 - fy * q3 + fz * q2; h12 = fx * q1 + fy * q2 + fz * q3; 2562 | h21 = fx * q3 + fy * q0 - fz * q1; h22 = fx * q2 - fy * q1 - fz * q0; 2563 | Hk.dd[0] = h11 * 2, Hk.dd[1] = h12 * 2, Hk.dd[2] = -h22 * 2, Hk.dd[3] = -h21 * 2; 2564 | Hk.dd[7] = h21 * 2, Hk.dd[8] = h22 * 2, Hk.dd[9] = h12 * 2, Hk.dd[10] = h11 * 2; 2565 | /* CVect3 magH = Cnb*mag; 2566 | double C11=Cnb.e11, C01=Cnb.e01, CC=C11*C11+C01*C01; 2567 | if(normXY(magH)>0.01 && CC>0.25) // CC>0.25 <=> pitch<60deg 2568 | { 2569 | double f2=2.0/CC; 2570 | Hk.dd[14] = (q3*C11+q0*C01)*f2, Hk.dd[15] = (-q2*C11-q1*C01)*f2, Hk.dd[16] = (-q1*C11+q2*C01)*f2, Hk.dd[17] = (q0*C11-q3*C01)*f2; 2571 | Zk.dd[2] = atan2(magH.i, magH.j); 2572 | } 2573 | else 2574 | { 2575 | Hk.dd[14] = Hk.dd[15] = Hk.dd[16] = Hk.dd[17] = 0.0; 2576 | Zk.dd[2] = 0.0; 2577 | }*/ 2578 | 2579 | SetMeasFlag(0x03); 2580 | TimeUpdate(ts); 2581 | MeasUpdate(); 2582 | XPConstrain(); 2583 | normlize((CQuat*)&Xk.dd[0]); 2584 | Cnb = q2mat(*(CQuat*)&Xk.dd[0]); 2585 | } 2586 | 2587 | #endif // PSINS_AHRS_MEMS 2588 | 2589 | /****************************** File Read or Write *********************************/ 2590 | #ifdef PSINS_IO_FILE 2591 | #pragma message(" PSINS_IO_FILE") 2592 | 2593 | #include 2594 | #include 2595 | char* time2fname(void) 2596 | { 2597 | static char PSINSfname[32]; 2598 | time_t tt; time(&tt); 2599 | tm *Time = localtime(&tt); 2600 | strftime(PSINSfname, 32, "PSINS%Y%m%d_%H%M%S.bin", Time); 2601 | return PSINSfname; 2602 | } 2603 | 2604 | char CFileRdWt::dirIn[256] = { 0 }, CFileRdWt::dirOut[256] = { 0 }; 2605 | 2606 | void CFileRdWt::Dir(const char *dirI, const char *dirO) // set dir 2607 | { 2608 | int len = strlen(dirI); 2609 | memcpy(dirIn, dirI, len); 2610 | if (dirIn[len - 1] != '\\') { dirIn[len] = '\\'; dirIn[len + 1] = '\0'; } 2611 | if (dirO) 2612 | { 2613 | len = strlen(dirO); 2614 | memcpy(dirOut, dirO, len); 2615 | if (dirOut[len - 1] != '\\') { dirOut[len] = '\\'; dirOut[len + 1] = '\0'; } 2616 | } 2617 | else 2618 | memcpy(dirOut, dirIn, strlen(dirIn)); 2619 | if (access(dirIn, 0) == -1) dirIn[0] = '\0'; 2620 | if (access(dirOut, 0) == -1) dirOut[0] = '\0'; 2621 | } 2622 | 2623 | CFileRdWt::CFileRdWt() 2624 | { 2625 | f = 0; 2626 | } 2627 | 2628 | CFileRdWt::CFileRdWt(const char *fname0, int columns0) 2629 | { 2630 | f = 0; 2631 | Init(fname0, columns0); 2632 | memset(buff, 0, sizeof(buff)); 2633 | } 2634 | 2635 | CFileRdWt::~CFileRdWt() 2636 | { 2637 | if (f) { fclose(f); f = (FILE*)NULL; } 2638 | } 2639 | 2640 | void CFileRdWt::Init(const char *fname0, int columns0) 2641 | { 2642 | fname[0] = '\0'; 2643 | int findc = 0, len0 = strlen(fname0); 2644 | for (int i = 0; i < len0; i++) { if (fname0[i] == '\\') { findc = 1; break; } } 2645 | columns = columns0; 2646 | if (columns == 0) // file write 2647 | { 2648 | if (dirOut[0] != 0 && findc == 0) { strcat(fname, dirOut); } 2649 | } 2650 | else // file read 2651 | { 2652 | if (dirIn[0] != 0 && findc == 0) { strcat(fname, dirIn); } 2653 | } 2654 | strcat(fname, fname0); 2655 | if (f) this->~CFileRdWt(); 2656 | if (columns == 0) // bin file write 2657 | { 2658 | f = fopen(fname, "wb"); 2659 | } 2660 | else if (columns < 0) // bin file read 2661 | { 2662 | f = fopen(fname, "rb"); 2663 | } 2664 | else if (columns > 0) // txt file read 2665 | { 2666 | f = fopen(fname, "rt"); 2667 | if (!f) return; 2668 | fpos_t pos; 2669 | while (1) // skip txt-file comments 2670 | { 2671 | //pos = ftell(f); 2672 | fgetpos(f,&pos); 2673 | fgets(line, sizeof(line), f); 2674 | if (feof(f)) break; 2675 | int allSpace = 1, allDigital = 1; 2676 | for (int i = 0; i < sizeof(line); i++) 2677 | { 2678 | char c = line[i]; 2679 | if (c == '\n') break; 2680 | if (c != ' ') allSpace = 0; 2681 | if (!(c == ' ' || c == ',' || c == ';' || c == ':' || c == '+' || c == '-' || c == '.' || c == '\t' 2682 | || c == 'e' || c == 'E' || c == 'd' || c == 'D' || ('9' >= c && c >= '0'))) 2683 | allDigital = 0; 2684 | } 2685 | if (!allSpace && allDigital) break; 2686 | } 2687 | fsetpos(f, &pos); 2688 | this->columns = columns; 2689 | for (int i = 0; i < columns; i++) 2690 | { 2691 | sstr[4 * i + 0] = '%', sstr[4 * i + 1] = 'l', sstr[4 * i + 2] = 'f', sstr[4 * i + 3] = ' ', sstr[4 * i + 4] = '\0'; 2692 | } 2693 | } 2694 | else 2695 | { 2696 | f = 0; 2697 | } 2698 | linelen = 0; 2699 | totsize = 0; 2700 | } 2701 | 2702 | int CFileRdWt::load(int lines, BOOL txtDelComma) 2703 | { 2704 | if (columns < 0) // bin file read 2705 | { 2706 | if (lines > 1) 2707 | fseek(f, (lines - 1)*(-columns) * sizeof(double), SEEK_CUR); 2708 | fread(buff, -columns, sizeof(double), f); 2709 | } 2710 | else // txt file read 2711 | { 2712 | for (int i = 0; i < lines; i++) fgets(line, sizeof(line), f); 2713 | if (txtDelComma) 2714 | { 2715 | for (char *pc = line, *pend = line + sizeof(line); pc < pend; pc++) 2716 | { 2717 | if (*pc == ',' || *pc == ';' || *pc == ':' || *pc == '\t:') *pc = ' '; 2718 | else if (*pc == '\0') break; 2719 | } 2720 | } 2721 | if (columns < 10) 2722 | sscanf(line, sstr, 2723 | &buff[0], &buff[1], &buff[2], &buff[3], &buff[4], &buff[5], &buff[6], &buff[7], &buff[8], &buff[9] 2724 | ); 2725 | else if (columns < 20) 2726 | sscanf(line, sstr, 2727 | &buff[0], &buff[1], &buff[2], &buff[3], &buff[4], &buff[5], &buff[6], &buff[7], &buff[8], &buff[9], 2728 | &buff[10], &buff[11], &buff[12], &buff[13], &buff[14], &buff[15], &buff[16], &buff[17], &buff[18], &buff[19] 2729 | ); 2730 | else if (columns < 40) 2731 | sscanf(line, sstr, 2732 | &buff[0], &buff[1], &buff[2], &buff[3], &buff[4], &buff[5], &buff[6], &buff[7], &buff[8], &buff[9], 2733 | &buff[10], &buff[11], &buff[12], &buff[13], &buff[14], &buff[15], &buff[16], &buff[17], &buff[18], &buff[19], 2734 | &buff[20], &buff[21], &buff[22], &buff[23], &buff[24], &buff[25], &buff[26], &buff[27], &buff[28], &buff[29], 2735 | &buff[30], &buff[31], &buff[32], &buff[33], &buff[34], &buff[35], &buff[36], &buff[37], &buff[38], &buff[39] 2736 | ); 2737 | } 2738 | linelen += lines; 2739 | if (feof(f)) return 0; 2740 | else return 1; 2741 | } 2742 | 2743 | int CFileRdWt::loadf32(int lines) // float32 bin file read 2744 | { 2745 | if (lines > 1) 2746 | fseek(f, (lines - 1)*(-columns) * sizeof(float), SEEK_CUR); 2747 | fread(buff32, -columns, sizeof(float), f); 2748 | for (int i = 0; i < -columns; i++) buff[i] = buff32[i]; 2749 | linelen += lines; 2750 | if (feof(f)) return 0; 2751 | else return 1; 2752 | } 2753 | 2754 | long CFileRdWt::load(BYTE *buf, long bufsize) // load bin file 2755 | { 2756 | long cur = ftell(f); 2757 | fseek(f, 0L, SEEK_END); 2758 | long rest = ftell(f) - cur; 2759 | fseek(f, cur, SEEK_SET); 2760 | if (bufsize > rest) bufsize = rest; 2761 | fread(buf, bufsize, 1, f); 2762 | return bufsize; 2763 | } 2764 | 2765 | long CFileRdWt::filesize(int opt) 2766 | { 2767 | long cur = ftell(f); 2768 | if (totsize == 0) 2769 | { 2770 | fseek(f, 0L, SEEK_END); 2771 | totsize = ftell(f); 2772 | fseek(f, cur, SEEK_SET); 2773 | } 2774 | remsize = totsize - cur; 2775 | return opt ? remsize : totsize; // opt==1 for remain_size, ==0 for total_size 2776 | } 2777 | 2778 | int CFileRdWt::getl(void) // txt file get a line 2779 | { 2780 | fgets(line, sizeof(line), f); 2781 | return strlen(line); 2782 | } 2783 | 2784 | CFileRdWt& CFileRdWt::operator<<(double d) 2785 | { 2786 | fwrite(&d, 1, sizeof(double), f); 2787 | return *this; 2788 | } 2789 | 2790 | CFileRdWt& CFileRdWt::operator<<(const CVect3 &v) 2791 | { 2792 | fwrite(&v, 1, sizeof(v), f); 2793 | return *this; 2794 | } 2795 | 2796 | CFileRdWt& CFileRdWt::operator<<(const CQuat &q) 2797 | { 2798 | fwrite(&q, 1, sizeof(q), f); 2799 | return *this; 2800 | } 2801 | 2802 | CFileRdWt& CFileRdWt::operator<<(const CMat3 &m) 2803 | { 2804 | fwrite(&m, 1, sizeof(m), f); 2805 | return *this; 2806 | } 2807 | 2808 | CFileRdWt& CFileRdWt::operator<<(const CVect &v) 2809 | { 2810 | fwrite(v.dd, v.clm*v.row, sizeof(double), f); 2811 | return *this; 2812 | } 2813 | 2814 | CFileRdWt& CFileRdWt::operator<<(const CMat &m) 2815 | { 2816 | fwrite(m.dd, m.clm*m.row, sizeof(double), f); 2817 | return *this; 2818 | } 2819 | 2820 | CFileRdWt& CFileRdWt::operator<<(const CRAvar &R) 2821 | { 2822 | fwrite(R.R0, R.nR0, sizeof(double), f); 2823 | return *this; 2824 | } 2825 | 2826 | CFileRdWt& CFileRdWt::operator<<(const CAligni0 &aln) 2827 | { 2828 | return *this << q2att(aln.qnb) << aln.vib0 << aln.Pi02 << aln.tk; 2829 | } 2830 | 2831 | CFileRdWt& CFileRdWt::operator<<(const CSINS &sins) 2832 | { 2833 | return *this << sins.att << sins.vn << sins.pos << sins.eb << sins.db << sins.tk; 2834 | } 2835 | 2836 | #ifdef PSINS_RMEMORY 2837 | CFileRdWt& CFileRdWt::operator<<(const CRMemory &m) 2838 | { 2839 | fwrite(m.pMemStart0, m.memLen, 1, f); 2840 | return *this; 2841 | } 2842 | #endif 2843 | 2844 | #ifdef PSINS_AHRS_MEMS 2845 | CFileRdWt& CFileRdWt::operator<<(const CMahony &ahrs) 2846 | { 2847 | return *this << m2att(ahrs.Cnb) << ahrs.exyzInt << ahrs.tk; 2848 | } 2849 | 2850 | CFileRdWt& CFileRdWt::operator<<(const CQEAHRS &ahrs) 2851 | { 2852 | return *this << m2att(ahrs.Cnb) << *(CVect3*)&ahrs.Xk.dd[4] << diag(ahrs.Pk) << ahrs.kftk; 2853 | } 2854 | #endif 2855 | 2856 | #ifdef PSINS_UART_PUSH_POP 2857 | CFileRdWt& CFileRdWt::operator<<(const CUartPP &uart) 2858 | { 2859 | fwrite(uart.popbuf, uart.frameLen, sizeof(char), f); 2860 | return *this; 2861 | } 2862 | #endif 2863 | 2864 | CFileRdWt& CFileRdWt::operator<<(CKalman &kf) 2865 | { 2866 | *this << kf.Xk << diag(kf.Pk) << kf.Rt << (double)kf.measflaglog << kf.kftk; 2867 | kf.measflaglog = 0; 2868 | return *this; 2869 | } 2870 | 2871 | CFileRdWt& CFileRdWt::operator>>(double &d) 2872 | { 2873 | fread(&d, 1, sizeof(double), f); 2874 | return *this; 2875 | } 2876 | 2877 | CFileRdWt& CFileRdWt::operator>>(CVect3 &v) 2878 | { 2879 | fread(&v, 1, sizeof(v), f); 2880 | return *this; 2881 | } 2882 | 2883 | CFileRdWt& CFileRdWt::operator>>(CQuat &q) 2884 | { 2885 | fread(&q, 1, sizeof(q), f); 2886 | return *this; 2887 | } 2888 | 2889 | CFileRdWt& CFileRdWt::operator>>(CMat3 &m) 2890 | { 2891 | fread(&m, 1, sizeof(m), f); 2892 | return *this; 2893 | } 2894 | 2895 | CFileRdWt& CFileRdWt::operator>>(CVect &v) 2896 | { 2897 | fread(v.dd, v.clm*v.row, sizeof(double), f); 2898 | return *this; 2899 | } 2900 | 2901 | CFileRdWt& CFileRdWt::operator>>(CMat &m) 2902 | { 2903 | fread(m.dd, m.clm*m.row, sizeof(double), f); 2904 | return *this; 2905 | } 2906 | 2907 | #endif // PSINS_IO_FILE 2908 | 2909 | #ifdef PSINS_RMEMORY 2910 | #pragma message(" PSINS_RMEMORY") 2911 | 2912 | CRMemory::CRMemory(long recordNum, int recordLen0) 2913 | { 2914 | BYTE *pb = (BYTE*)malloc(recordNum*recordLen0); 2915 | *this = CRMemory(pb, recordNum*recordLen0, recordLen0); 2916 | pMemStart0 = pMemStart; 2917 | } 2918 | 2919 | CRMemory::CRMemory(BYTE *pMem, long memLen0, int recordLen0) 2920 | { 2921 | psinsassert(recordLen0 <= MAX_RECORD_BYTES); 2922 | pMemStart0 = NULL; 2923 | pMemStart = pMemPush = pMemPop = pMem; 2924 | pMemEnd = pMemStart + memLen0; 2925 | pushLen = popLen = recordLen = recordLen0; 2926 | memLen = memLen0; 2927 | dataLen = 0; 2928 | } 2929 | 2930 | CRMemory::~CRMemory() 2931 | { 2932 | if (pMemStart0) { free(pMemStart0); pMemStart0 = NULL; } 2933 | } 2934 | 2935 | BYTE CRMemory::pop(BYTE *p) 2936 | { 2937 | if (dataLen == 0) return 0; 2938 | popLen = recordLen == 0 ? *pMemPop : recordLen; 2939 | if (p == (BYTE*)NULL) p = popBuf; 2940 | BYTE i; 2941 | for (i = 0; i < popLen; i++, dataLen--) 2942 | { 2943 | *p++ = *pMemPop++; 2944 | if (pMemPop >= pMemEnd) pMemPop = pMemStart; 2945 | } 2946 | return i; 2947 | } 2948 | 2949 | BYTE* CRMemory::get(int iframe) 2950 | { 2951 | return &pMemStart[popLen*iframe]; 2952 | } 2953 | 2954 | BOOL CRMemory::push(const BYTE *p) 2955 | { 2956 | BOOL res = 1; 2957 | if (p == (BYTE*)NULL) p = pushBuf; 2958 | pushLen = recordLen == 0 ? *p : recordLen; 2959 | psinsassert(pushLen <= MAX_RECORD_BYTES); 2960 | for (BYTE i = 0; i < pushLen; i++, dataLen++) 2961 | { 2962 | *pMemPush++ = *p++; 2963 | if (pMemPush >= pMemEnd) pMemPush = pMemStart; 2964 | if (pMemPush == pMemPop) { res = 0; pop(); } 2965 | } 2966 | return res; 2967 | } 2968 | 2969 | #endif // PSINS_RMEMORY 2970 | 2971 | #ifdef PSINS_VC_AFX_HEADER 2972 | 2973 | CVCFileFind::CVCFileFind(char *path, char *filetype) 2974 | { 2975 | finder.FindFile(strcat(strcpy(fname, path), filetype)); 2976 | lastfile = 0; 2977 | } 2978 | 2979 | char *CVCFileFind::FindNextFile(void) 2980 | { 2981 | if (lastfile) return 0; 2982 | if (!finder.FindNextFile()) lastfile = 1; 2983 | CString strname = finder.GetFileName(); 2984 | sprintf(fname, "%s", strname.LockBuffer()); 2985 | return fname; 2986 | } 2987 | 2988 | #endif // PSINS_VC_AFX_HEADER 2989 | 2990 | /*************************** function AlignCoarse *********************************/ 2991 | CVect3 Alignsb(CVect3 wmm, CVect3 vmm, double latitude) 2992 | { 2993 | double T11, T12, T13, T21, T22, T23, T31, T32, T33; 2994 | double cl = cos(latitude), tl = tan(latitude), nn; 2995 | CVect3 wbib = wmm / norm(wmm), fb = vmm / norm(vmm); 2996 | T31 = fb.i, T32 = fb.j, T33 = fb.k; 2997 | T21 = wbib.i / cl - T31 * tl, T22 = wbib.j / cl - T32 * tl, T23 = wbib.k / cl - T33 * tl; nn = sqrt(T21*T21 + T22 * T22 + T23 * T23); T21 /= nn, T22 /= nn, T23 /= nn; 2998 | T11 = T22 * T33 - T23 * T32, T12 = T23 * T31 - T21 * T33, T13 = T21 * T32 - T22 * T31; nn = sqrt(T11*T11 + T12 * T12 + T13 * T13); T11 /= nn, T12 /= nn, T13 /= nn; 2999 | CMat3 Cnb(T11, T12, T13, T21, T22, T23, T31, T32, T33); 3000 | return m2att(Cnb); 3001 | } 3002 | 3003 | /*************************** CAligni0 *********************************/ 3004 | CAligni0::CAligni0(const CVect3 &pos0, const CVect3 &vel0, int velAid0) 3005 | { 3006 | eth.Update(pos0); 3007 | this->vel0 = vel0, velAid = velAid0; 3008 | tk = 0; 3009 | t0 = t1 = 10, t2 = 0; 3010 | wmm = vmm = vib0 = vi0 = Pib01 = Pib02 = Pi01 = Pi02 = O31; 3011 | qib0b = CQuat(1.0); 3012 | } 3013 | 3014 | CQuat CAligni0::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts, const CVect3 &vel) 3015 | { 3016 | double nts = nSamples * ts; 3017 | imu.Update(pwm, pvm, nSamples); 3018 | wmm = wmm + imu.phim; vmm = vmm + imu.dvbm; 3019 | // vtmp = qib0b * (vm + 1/2 * wm X vm) 3020 | CVect3 vtmp = qib0b * imu.dvbm; 3021 | // vtmp1 = qni0' * [dvn+(wnin+wnie)Xvn-gn] * ts; 3022 | tk += nts; 3023 | CMat3 Ci0n = pos2Cen(CVect3(eth.pos.i, eth.wie*tk, 0.0)); 3024 | CVect3 vtmp1 = Ci0n * (-eth.gn*nts); 3025 | if (velAid > 0) 3026 | { 3027 | CVect3 dv = vel - vel0; vel0 = vel; 3028 | if (velAid == 1) vtmp1 += Ci0n * dv; // for GPS vn-aided 3029 | else if (velAid == 2) vtmp -= qib0b * dv + imu.phim*vel; // for OD vb-aided 3030 | } 3031 | // Pib02 = Pib02 + vib0*ts, Pi02 = Pi02 + vi0*ts 3032 | vib0 = vib0 + vtmp, vi0 = vi0 + vtmp1; 3033 | Pib02 = Pib02 + vib0 * nts, Pi02 = Pi02 + vi0 * nts; 3034 | // 3035 | if (++t2 > 3 * t0) 3036 | { 3037 | t0 = t1, Pib01 = tmpPib0, Pi01 = tmpPi0; 3038 | } 3039 | else if (t2 > 2 * t0 && t1 == t0) 3040 | { 3041 | t1 = t2, tmpPib0 = Pib02, tmpPi0 = Pi02; 3042 | } 3043 | // 3044 | qib0b = qib0b * rv2q(imu.phim); 3045 | // qnb=qni0*qiib0*qib0b 3046 | qnbsb = a2qua(Alignsb(wmm, vmm, eth.pos.i)); 3047 | if (t2 < 100) 3048 | { 3049 | qnb0 = qnb = CQuat(1.0); 3050 | } 3051 | else if (t2 < 1000) 3052 | { 3053 | qnb0 = qnb = qnbsb; 3054 | } 3055 | else 3056 | { 3057 | CQuat qi0ib0 = m2qua(dv2att(Pi01, Pi02, Pib01, Pib02)); 3058 | qnb0 = (~m2qua(pos2Cen(CVect3(eth.pos.i, 0.0, 0.0))))*qi0ib0; 3059 | qnb = (~m2qua(Ci0n))*qi0ib0*qib0b; 3060 | } 3061 | return qnb; 3062 | } 3063 | 3064 | /*************************** CAlignkf *********************************/ 3065 | CAlignkf::CAlignkf(void) :CSINSTDKF(15, 6) 3066 | { 3067 | } 3068 | 3069 | void CAlignkf::Init(const CSINS &sins0) 3070 | { 3071 | CSINSKF::Init(sins0, -1); 3072 | Pmax.Set2(10.0*glv.deg, 10.0*glv.deg, 30.0*glv.deg, 50.0, 50.0, 50.0, 1.0e4 / glv.Re, 1.0e4 / glv.Re, 1.0e4, 3073 | 10.0*glv.dph, 10.0*glv.dph, 10.0*glv.dph, 10.0*glv.mg, 10.0*glv.mg, 10.0*glv.mg); 3074 | Pmin.Set2(0.01*glv.min, 0.01*glv.min, 0.1*glv.min, 0.001, 0.001, 0.01, 1.0 / glv.Re, 1.0 / glv.Re, 0.1, 3075 | 0.001*glv.dph, 0.001*glv.dph, 0.001*glv.dph, .50*glv.ug, .50*glv.ug, 1.50*glv.ug); 3076 | Pk.SetDiag2(.10*glv.deg, .10*glv.deg, 10.0*glv.deg, 1.0, 1.0, 1.0, 100.0 / glv.Re, 100.0 / glv.Re, 100.0, 3077 | 0.05*glv.dph, 0.05*glv.dph, 0.01*glv.dph, .10*glv.mg, .10*glv.mg, .10*glv.mg); 3078 | Qt.Set2(0.001*glv.dpsh, 0.001*glv.dpsh, 0.001*glv.dpsh, 1.0*glv.ugpsHz, 1.0*glv.ugpsHz, 1.0*glv.ugpsHz, 0.0, 0.0, 0.0, 3079 | 0.0*glv.dphpsh, 0.0*glv.dphpsh, 0.0*glv.dphpsh, 0.0*glv.ugpsh, 0.0*glv.ugpsh, 0.0*glv.ugpsh); 3080 | Xmax.Set(INF, INF, INF, INF, INF, INF, INF, INF, INF, 0.1*glv.dph, 0.1*glv.dph, 0.1*glv.dph, 200.0*glv.ug, 200.0*glv.ug, 200.0*glv.ug); 3081 | Rt.Set2(0.1, 0.1, 0.1, 100.0 / glv.Re, 100.0 / glv.Re, 300.0); 3082 | // Rmax = Rt * 100; Rmin = Rt*0.01; Rb = 0.9; 3083 | FBTau.Set(1.0, 1.0, 10.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0); 3084 | mvnk = 0; 3085 | mvnts = 0.0; 3086 | mvn = O31; 3087 | pos0 = sins.pos; 3088 | } 3089 | 3090 | int CAlignkf::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts) 3091 | { 3092 | mvnk++; 3093 | mvnts += nSamples * ts; 3094 | mvn += sins.vn; 3095 | if (mvnts > .1) 3096 | { 3097 | *(CVect3*)&Zk.dd[0] = mvn * (1.0 / mvnk); 3098 | if (norm(sins.wnb) < 0.1*glv.dps) 3099 | SetMeasFlag(0007); 3100 | mvnk = 0; 3101 | mvnts = 0.0; 3102 | mvn = O31; 3103 | sins.pos = pos0; 3104 | } 3105 | int res = TDUpdate(pwm, pvm, nSamples, ts, 5); 3106 | return res; 3107 | } 3108 | 3109 | int CAlignkf::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts, const CVect3 &vnr) 3110 | { 3111 | if (norm(vnr) > 0) 3112 | { 3113 | *(CVect3*)&Zk.dd[0] = sins.vn - vnr; 3114 | if (norm(sins.wnb) < 1.1*glv.dps) 3115 | SetMeasFlag(0007); 3116 | } 3117 | int res = TDUpdate(pwm, pvm, nSamples, ts, 5); 3118 | return res; 3119 | } 3120 | 3121 | /*************************** class CUartPP *********************************/ 3122 | #ifdef PSINS_UART_PUSH_POP 3123 | 3124 | #pragma message(" PSINS_UART_PUSH_POP") 3125 | 3126 | CUartPP::CUartPP(int frameLen0, unsigned short head0) 3127 | { 3128 | popIdx = 0; 3129 | pushIdx = 1; 3130 | unsigned char *p = (unsigned char*)&head0; 3131 | head[0] = p[1], head[1] = p[0]; 3132 | // *(unsigned short*)head = head0; 3133 | frameLen = frameLen0; 3134 | csflag = 1, cs0 = 4, cs1 = frameLen - 1, css = 2; 3135 | overflow = getframe = 0; 3136 | } 3137 | 3138 | BOOL CUartPP::checksum(const unsigned char *pc) 3139 | { 3140 | chksum = 0; 3141 | if (csflag == 0) return 1; 3142 | for (int i = cs0; i <= cs1; i++) 3143 | { 3144 | if (csflag == 1) chksum += pc[i]; 3145 | else if (csflag == 2) chksum ^= pc[i]; 3146 | } 3147 | return chksum == pc[css]; 3148 | } 3149 | 3150 | int CUartPP::nextIdx(int idx) 3151 | { 3152 | return idx >= UARTBUFLEN - 1 ? 0 : idx + 1; 3153 | } 3154 | 3155 | int CUartPP::push(const unsigned char *buf0, int len) 3156 | { 3157 | int overflowtmp = 0; 3158 | for (int i = 0; i < len; i++) 3159 | { 3160 | buf[pushIdx] = buf0[i]; 3161 | pushIdx = nextIdx(pushIdx); 3162 | if (pushIdx == popIdx) { 3163 | overflow++; 3164 | overflowtmp++; 3165 | popIdx = nextIdx(popIdx); 3166 | } 3167 | } 3168 | return overflowtmp; 3169 | } 3170 | 3171 | int CUartPP::pop(unsigned char *buf0) 3172 | { 3173 | int getframetmp = 0; 3174 | while (1) 3175 | { 3176 | if ((pushIdx > popIdx&&popIdx + frameLen < pushIdx) || (pushIdx < popIdx&&popIdx + frameLen < pushIdx + UARTBUFLEN)) 3177 | { 3178 | int popIdx1 = nextIdx(popIdx); 3179 | if (buf[popIdx] == head[0] && buf[popIdx1] == head[1]) 3180 | { 3181 | if (!buf0) buf0 = &popbuf[0]; 3182 | for (int i = 0; i < frameLen; i++) 3183 | { 3184 | buf0[i] = buf[popIdx]; 3185 | popIdx = nextIdx(popIdx); 3186 | } 3187 | if (checksum(buf0)) // checksum 3188 | { 3189 | getframe++; 3190 | getframetmp = 1; 3191 | break; 3192 | } 3193 | else 3194 | popIdx = popIdx1; 3195 | } 3196 | else 3197 | { 3198 | popIdx = popIdx1; 3199 | } 3200 | } 3201 | else 3202 | break; 3203 | } 3204 | return getframetmp; 3205 | } 3206 | #endif 3207 | 3208 | unsigned short swap16(unsigned short ui16) 3209 | { 3210 | unsigned char *p = (unsigned char*)&ui16, c; 3211 | c = p[0]; p[0] = p[1]; p[1] = c; 3212 | return ui16; 3213 | } 3214 | 3215 | unsigned char* swap24(unsigned char* puc3, unsigned char* pres) 3216 | { 3217 | static unsigned char resc[3]; 3218 | if (pres == NULL) pres = resc; 3219 | pres[0] = puc3[2]; pres[1] = puc3[1]; pres[2] = puc3[0]; 3220 | return pres; 3221 | } 3222 | 3223 | unsigned int swap32(unsigned int ui32) 3224 | { 3225 | unsigned char *p = (unsigned char*)&ui32, c; 3226 | c = p[0]; p[0] = p[3]; p[3] = c; 3227 | c = p[1]; p[1] = p[2]; p[2] = c; 3228 | return ui32; 3229 | } 3230 | 3231 | unsigned long swap64(unsigned long ui64) 3232 | { 3233 | unsigned char *p = (unsigned char*)&ui64, c; 3234 | c = p[0]; p[0] = p[7]; p[7] = c; 3235 | c = p[1]; p[1] = p[6]; p[6] = c; 3236 | c = p[2]; p[2] = p[5]; p[5] = c; 3237 | c = p[3]; p[3] = p[4]; p[4] = c; 3238 | return ui64; 3239 | } 3240 | 3241 | unsigned char* int24(unsigned char *pchar3, int int32) 3242 | { 3243 | unsigned char *p = (unsigned char*)&int32; 3244 | *pchar3++ = *p++, *pchar3++ = *p++, *pchar3 = *p; 3245 | return pchar3 - 2; 3246 | } 3247 | 3248 | int diffint24(const unsigned char *pc1, const unsigned char *pc0) 3249 | { 3250 | int i1, i0; 3251 | unsigned char *p1 = (unsigned char*)&i1, *p0 = (unsigned char*)&i0; 3252 | *p1++ = 0, *p1++ = *pc1++, *p1++ = *pc1++, *p1 = *pc1; 3253 | *p0++ = 0, *p0++ = *pc0++, *p0++ = *pc0++, *p0 = *pc0; 3254 | return (i1 - i0) / 256; 3255 | } 3256 | 3257 | #ifdef PSINS_psinsassert 3258 | 3259 | #pragma message(" psinsassert();") 3260 | 3261 | BOOL psinsassert(BOOL b) 3262 | { 3263 | int res; 3264 | 3265 | if (b) { 3266 | res = 1; 3267 | } 3268 | else { 3269 | res = 0; 3270 | } 3271 | return res; 3272 | } 3273 | 3274 | #endif 3275 | 3276 | double r2dm(double r) // rad to deg/min, eg. 1234.56 = 12deg+34.56min 3277 | { 3278 | int sgn = 1; 3279 | if (r < 0.0) { r = -r; sgn = 0; } 3280 | double deg = r / DEG; 3281 | int ideg = (int)deg; 3282 | double dm = ideg * 100 + (deg - ideg)*60.0; 3283 | return sgn ? dm : -dm; 3284 | } 3285 | 3286 | double dm2r(double dm) 3287 | { 3288 | int sgn = 1; 3289 | if (dm < 0.0) { dm = -dm; sgn = 0; } 3290 | int ideg = (int)(dm / 100); 3291 | double r = ideg * DEG + (dm - ideg * 100)*(DEG / 60); 3292 | return sgn ? r : -r; 3293 | } 3294 | 3295 | //BOOL IsZero(double f, double eps) 3296 | //{ 3297 | // return (f-eps); 3298 | //} 3299 | 3300 | // determine the sign of 'val' with the sensitivity of 'eps' 3301 | int sign(double val, double eps) 3302 | { 3303 | int s; 3304 | 3305 | if (val < -eps) 3306 | { 3307 | s = -1; 3308 | } 3309 | else if (val > eps) 3310 | { 3311 | s = 1; 3312 | } 3313 | else 3314 | { 3315 | s = 0; 3316 | } 3317 | return s; 3318 | } 3319 | 3320 | // set double value 'val' between range 'minVal' and 'maxVal' 3321 | double range(double val, double minVal, double maxVal) 3322 | { 3323 | double res; 3324 | 3325 | if (val < minVal) 3326 | { 3327 | res = minVal; 3328 | } 3329 | else if (val > maxVal) 3330 | { 3331 | res = maxVal; 3332 | } 3333 | else 3334 | { 3335 | res = val; 3336 | } 3337 | return res; 3338 | } 3339 | 3340 | double atan2Ex(double y, double x) 3341 | { 3342 | double res; 3343 | 3344 | if ((sign(y) == 0) && (sign(x) == 0)) 3345 | { 3346 | res = 0.0; 3347 | } 3348 | else 3349 | { 3350 | res = atan2(y, x); 3351 | } 3352 | return res; 3353 | } 3354 | 3355 | double diffYaw(double yaw, double yaw0) 3356 | { 3357 | double dyaw = yaw - yaw0; 3358 | if (dyaw >= PI) dyaw -= _2PI; 3359 | else if (dyaw <= -PI) dyaw += _2PI; 3360 | return dyaw; 3361 | } 3362 | 3363 | double MKQt(double sR, double tau) 3364 | { 3365 | return sR * sR*2.0 / tau; 3366 | } 3367 | 3368 | CVect3 MKQt(const CVect3 &sR, const CVect3 &tau) 3369 | { 3370 | return CVect3(sR.i*sR.i*2.0 / tau.i, sR.j*sR.j*2.0 / tau.j, sR.k*sR.k*2.0 / tau.k); 3371 | } 3372 | 3373 | double randn(double mu, double sigma) 3374 | { 3375 | #define NSUM 25 3376 | static double ssgm = sqrt(NSUM / 12.0); 3377 | double x = 0; 3378 | for (int i = 0; i < NSUM; i++) 3379 | { 3380 | x += (double)rand(); 3381 | } 3382 | x /= RAND_MAX; 3383 | x -= NSUM / 2.0; x += mu; 3384 | x /= ssgm; x *= sigma; 3385 | return x; 3386 | } 3387 | 3388 | CVect3 randn(const CVect3 &mu, const CVect3 &sigma) 3389 | { 3390 | return CVect3(randn(mu.i, sigma.i), randn(mu.j, sigma.j), randn(mu.k, sigma.k)); 3391 | } 3392 | ///////////////////////////////////////new add//////////////////////////////////////////////// 3393 | 3394 | std::ofstream & operator<<(std::ofstream &out, CVect3 &A){ 3395 | out << A.i <<"\t"<< A.j <<"\t" << A.k <<"\t"; 3396 | return out; 3397 | } 3398 | 3399 | std::ofstream & operator<<(std::ofstream &out, CVect &A){ 3400 | for (int j=0;j