├── DataSet └── Low-accuracy │ ├── AHRS │ └── AHRS_StaticData.txt │ ├── GNSS │ └── GPS_IMU-1M.txt │ └── Gyro │ └── single axis gyroscope.txt ├── Document └── PSINS ClassGraph.png ├── Source ├── Algorithm │ ├── CMakeLists.txt │ ├── PSINS.cpp │ └── PSINS.h └── CMakeLists.txt └── readme.txt /Document/PSINS ClassGraph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Cattle521/PSINS-Develop-Group/d249b25b445ec8787dff94b73201577a2038a41d/Document/PSINS ClassGraph.png -------------------------------------------------------------------------------- /Source/Algorithm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #Templates are created by Cattle 2018.12.2 2 | 3 | SET (LIB_PSINS_SRC PSINS.cpp) 4 | 5 | SET (LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib) 6 | 7 | ADD_LIBRARY (PSINS SHARED ${LIB_PSINS_SRC}) 8 | 9 | ADD_LIBRARY (PSINS_static STATIC ${LIB_PSINS_SRC}) 10 | 11 | 12 | SET_TARGET_PROPERTIES (PSINS_static PROPERTIES OUTPUT_NAME "PSINS") 13 | 14 | 15 | 16 | GET_TARGET_PROPERTY (OUTPUT_VALUE PSINS_static OUTPUT_NAME) 17 | 18 | MESSAGE (STATUS "This is the PSINS_static OUTPUT_NAME: " ${OUTPUT_VALUE}) 19 | 20 | SET_TARGET_PROPERTIES (PSINS_static PROPERTIES CLEAN_DIRECT_OUTPUT 1) 21 | 22 | SET_TARGET_PROPERTIES (PSINS PROPERTIES CLEAN_DIRECT_OUTPUT 1) 23 | 24 | SET_TARGET_PROPERTIES (PSINS PROPERTIES VERSION 1.2 SOVERSION 1) 25 | 26 | 27 | -------------------------------------------------------------------------------- /Source/Algorithm/PSINS.cpp: -------------------------------------------------------------------------------- 1 | //#include "stdafx.h" 2 | #include "PSINS.h" 3 | 4 | const CVect3 I31(1.0), O31(0.0), Ipos(1.0/RE,1.0/RE,1.0); 5 | const CQuat qI(1.0,0.0,0.0,0.0); 6 | const CMat3 I33(1,0,0, 0,1,0, 0,0,1), O33(0,0,0, 0,0,0, 0,0,0); 7 | const CVect On1(MMD,0.0), O1n=~On1; 8 | const CGLV glv; 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 | mpsh = 1/sqrt(hur); 31 | mpspsh = 1/1/sqrt(hur); 32 | ppmpsh = ppm/sqrt(hur); 33 | secpsh = sec/sqrt(hur); 34 | } 35 | 36 | /*************************** class CVect3 *********************************/ 37 | CVect3::CVect3(void) 38 | { 39 | } 40 | 41 | CVect3::CVect3(double xyz) 42 | { 43 | i=j=k=xyz; 44 | } 45 | 46 | CVect3::CVect3(double xx, double yy, double zz) 47 | { 48 | i=xx, j=yy, k=zz; 49 | } 50 | 51 | CVect3::CVect3(const double *pdata) 52 | { 53 | i=*pdata++, j=*pdata++, k=*pdata; 54 | } 55 | 56 | BOOL IsZero(const CVect3 &v, double eps) 57 | { 58 | return (v.i-eps && v.j-eps && v.k-eps); 59 | } 60 | 61 | BOOL sZeroXY(const CVect3 &v, double eps) 62 | { 63 | return (v.i-eps && v.j-eps); 64 | } 65 | 66 | BOOL IsNaN(const CVect3 &v) 67 | { 68 | return 0; //(_isnan(i) || _isnan(j) || _isnan(k)); 69 | } 70 | 71 | CVect3 CVect3::operator+(const CVect3 &v) const 72 | { 73 | return CVect3(this->i+v.i, this->j+v.j, this->k+v.k); 74 | } 75 | 76 | CVect3 CVect3::operator-(const CVect3 &v) const 77 | { 78 | return CVect3(this->i-v.i, this->j-v.j, this->k-v.k); 79 | } 80 | 81 | CVect3 CVect3::operator*(const CVect3 &v) const 82 | { 83 | 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); 84 | } 85 | 86 | CVect3 CVect3::operator*(double f) const 87 | { 88 | return CVect3(i*f, j*f, k*f); 89 | } 90 | 91 | CVect3 CVect3::operator*(const CMat3 &m) const 92 | { 93 | 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); 94 | } 95 | 96 | CVect3 CVect3::operator/(double f) const 97 | { 98 | return CVect3(i/f, j/f, k/f); 99 | } 100 | 101 | CVect3 CVect3::operator/(const CVect3 &v) const 102 | { 103 | return CVect3(i/v.i, j/v.j, k/v.k); 104 | } 105 | 106 | CVect3& CVect3::operator=(double f) 107 | { 108 | i = j = k = f; 109 | return *this; 110 | } 111 | 112 | CVect3& CVect3::operator=(const double *pf) 113 | { 114 | i = *pf++, j = *pf++, k = *pf; 115 | return *this; 116 | } 117 | 118 | CVect3& CVect3::operator+=(const CVect3 &v) 119 | { 120 | i += v.i, j += v.j, k += v.k; 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*=(double f) 131 | { 132 | i *= f, j *= f, k *= f; 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/=(const CVect3 &v) 143 | { 144 | i /= v.k, j /= v.j, k /= v.k; 145 | return *this; 146 | } 147 | 148 | CVect3 operator*(double f, const CVect3 &v) 149 | { 150 | return CVect3(v.i*f, v.j*f, v.k*f); 151 | } 152 | 153 | CVect3 operator-(const CVect3 &v) 154 | { 155 | return CVect3(-v.i, -v.j, -v.k); 156 | } 157 | 158 | CVect3 sqrt(const CVect3 &v) 159 | { 160 | return CVect3(sqrt(v.i), sqrt(v.j), sqrt(v.k)); 161 | } 162 | 163 | CVect3 pow(const CVect3 &v, int k) 164 | { 165 | CVect3 pp = v; 166 | for(int i=1; i0.0 ? v.i : -v.i; 177 | res.j = v.j>0.0 ? v.j : -v.j; 178 | res.k = v.k>0.0 ? v.k : -v.k; 179 | return res; 180 | } 181 | 182 | double norm(const CVect3 &v) 183 | { 184 | return sqrt(v.i*v.i + v.j*v.j + v.k*v.k); 185 | } 186 | 187 | double normXY(const CVect3 &v) 188 | { 189 | return sqrt(v.i*v.i + v.j*v.j); 190 | } 191 | 192 | double dot(const CVect3 &v1, const CVect3 &v2) 193 | { 194 | return (v1.i*v2.i + v1.j*v2.j + v1.k*v2.k); 195 | } 196 | 197 | CQuat rv2q(const CVect3 &rv) 198 | { 199 | #define F1 ( 2 * 1) // define: Fk=2^k*k! 200 | #define F2 (F1*2 * 2) 201 | #define F3 (F2*2 * 3) 202 | #define F4 (F3*2 * 4) 203 | #define F5 (F4*2 * 5) 204 | double n2 = rv.i*rv.i+rv.j*rv.j+rv.k*rv.k, c, f; 205 | if(n2<(PI/180.0*PI/180.0)) // 0.017^2 206 | { 207 | double n4=n2*n2; 208 | c = 1.0 - n2*(1.0/F2) + n4*(1.0/F4); 209 | f = 0.5 - n2*(1.0/F3) + n4*(1.0/F5); 210 | } 211 | else 212 | { 213 | double n_2 = sqrt(n2)/2.0; 214 | c = cos(n_2); 215 | f = sin(n_2)/n_2*0.5; 216 | } 217 | return CQuat(c, f*rv.i, f*rv.j, f*rv.k); 218 | } 219 | 220 | CMat3 askew(const CVect3 &v) 221 | { 222 | return CMat3(0, -v.k, v.j, 223 | v.k, 0.0, -v.i, 224 | -v.j, v.i, 0); 225 | } 226 | 227 | CMat3 pos2Cen(const CVect3 &pos) 228 | { 229 | double si = sin(pos.i), ci = cos(pos.i), sj = sin(pos.j), cj = cos(pos.j); 230 | return CMat3( -sj, -si*cj, ci*cj, 231 | cj, -si*sj, ci*sj, 232 | 0, ci, si ); //Cen 233 | } 234 | 235 | CVect3 pp2vn(CVect3 &pos1, CVect3 &pos0, double ts, CEarth *pEth) 236 | { 237 | double sl, cl, sl2, sq, sq2, RMh, RNh, clRNh; 238 | if(pEth) 239 | { 240 | RMh = pEth->RMh; clRNh = pEth->clRNh; 241 | } 242 | else 243 | { 244 | sl=sin(pos0.i); cl=cos(pos0.i); sl2=sl*sl; 245 | sq = 1-glv.e2*sl2; sq2 = sqrt(sq); 246 | RMh = glv.Re*(1-glv.e2)/sq/sq2+pos0.k; 247 | RNh = glv.Re/sq2+pos0.k; clRNh = cl*RNh; 248 | } 249 | CVect3 vn = pos1 - pos0; 250 | return CVect3(vn.j*clRNh/ts, vn.i*RMh/ts, vn.k/ts); 251 | } 252 | 253 | double MagYaw(const CVect3 &mag, const CVect3 &att, double declination) 254 | { 255 | CVect3 attH(att.i, att.j, 0.0); 256 | CVect3 magH = a2mat(attH)*mag; 257 | double yaw = 0.0; 258 | if(attH.i<(80.0*DEG)&&attH.i>-(80.0*DEG)) 259 | { 260 | yaw = atan2Ex(magH.i, magH.j) + declination; 261 | if(yaw>PI) yaw -= _2PI; 262 | else if(yaw<-PI) yaw += _2PI; 263 | } 264 | return yaw; 265 | } 266 | 267 | CVect3 xyz2blh(const CVect3 &xyz) 268 | { 269 | double s = normXY(xyz), theta = atan2(xyz.k*glv.Re, s*glv.Rp), 270 | s3 = sin(theta), c3 = cos(theta); s3 = s3*s3*s3, c3 = c3*c3*c3; 271 | if(s<(6378137.0*1.0*DEG)) return O31; 272 | double L = atan2(xyz.j, xyz.i), B = atan2(xyz.k+glv.ep2*glv.Rp*s3, s-glv.e2*glv.Re*c3), 273 | sB = sin(B), cB = cos(B), N = glv.Re/sqrt(1-glv.e2*sB*sB); 274 | return CVect3(B, L, s/cB-N); 275 | } 276 | 277 | CVect3 blh2xyz(const CVect3 &blh) 278 | { 279 | double sB = sin(blh.i), cB = cos(blh.i), sL = sin(blh.j), cL = cos(blh.j), 280 | N = glv.Re/sqrt(1-glv.e2*sB*sB); 281 | return CVect3((N+blh.k)*cB*cL, (N+blh.k)*cB*sL, (N*(1-glv.e2)+blh.k)*sB); 282 | } 283 | 284 | CVect3 Vxyz2enu(const CVect3 &Vxyz, const CVect3 &pos) 285 | { 286 | return Vxyz*pos2Cen(pos); 287 | } 288 | 289 | /*************************** class CQuat *********************************/ 290 | CQuat::CQuat(void) 291 | { 292 | } 293 | 294 | CQuat::CQuat(double qq0, double qq1, double qq2, double qq3) 295 | { 296 | q0=qq0, q1=qq1, q2=qq2, q3=qq3; 297 | } 298 | 299 | CQuat::CQuat(const double *pdata) 300 | { 301 | q0=*pdata++, q1=*pdata++, q2=*pdata++, q3=*pdata++; 302 | } 303 | 304 | CQuat CQuat::operator+(const CVect3 &phi) const 305 | { 306 | CQuat qtmp = rv2q(-phi); 307 | return qtmp*(*this); 308 | } 309 | 310 | CQuat CQuat::operator-(const CVect3 &phi) const 311 | { 312 | CQuat qtmp = rv2q(phi); 313 | return qtmp*(*this); 314 | } 315 | 316 | CVect3 CQuat::operator-(CQuat &quat) const 317 | { 318 | CQuat dq; 319 | 320 | dq = quat*(~(*this)); 321 | if(dq.q0<0) 322 | { 323 | dq.q0=-dq.q0, dq.q1=-dq.q1, dq.q2=-dq.q2, dq.q3=-dq.q3; 324 | } 325 | double n2 = acos(dq.q0), f; 326 | if( sign(n2)!=0 ) 327 | { 328 | f = 2.0/(sin(n2)/n2); 329 | } 330 | else 331 | { 332 | f = 2.0; 333 | } 334 | return CVect3(dq.q1,dq.q2,dq.q3)*f; 335 | } 336 | 337 | CQuat CQuat::operator*(const CQuat &quat) const 338 | { 339 | CQuat qtmp; 340 | qtmp.q0 = q0*quat.q0 - q1*quat.q1 - q2*quat.q2 - q3*quat.q3; 341 | qtmp.q1 = q0*quat.q1 + q1*quat.q0 + q2*quat.q3 - q3*quat.q2; 342 | qtmp.q2 = q0*quat.q2 + q2*quat.q0 + q3*quat.q1 - q1*quat.q3; 343 | qtmp.q3 = q0*quat.q3 + q3*quat.q0 + q1*quat.q2 - q2*quat.q1; 344 | return qtmp; 345 | } 346 | 347 | CQuat& CQuat::operator*=(const CQuat &quat) 348 | { 349 | return (*this=*this*quat); 350 | } 351 | 352 | CQuat& CQuat::operator-=(const CVect3 &phi) 353 | { 354 | CQuat qtmp = rv2q(phi); 355 | return (*this=qtmp*(*this)); 356 | } 357 | 358 | CQuat operator~(const CQuat &q) 359 | { 360 | return CQuat(q.q0,-q.q1,-q.q2,-q.q3); 361 | } 362 | 363 | CVect3 CQuat::operator*(const CVect3 &v) const 364 | { 365 | CQuat qtmp; 366 | CVect3 vtmp; 367 | qtmp.q0 = - q1*v.i - q2*v.j - q3*v.k; 368 | qtmp.q1 = q0*v.i + q2*v.k - q3*v.j; 369 | qtmp.q2 = q0*v.j + q3*v.i - q1*v.k; 370 | qtmp.q3 = q0*v.k + q1*v.j - q2*v.i; 371 | vtmp.i = -qtmp.q0*q1 + qtmp.q1*q0 - qtmp.q2*q3 + qtmp.q3*q2; 372 | vtmp.j = -qtmp.q0*q2 + qtmp.q2*q0 - qtmp.q3*q1 + qtmp.q1*q3; 373 | vtmp.k = -qtmp.q0*q3 + qtmp.q3*q0 - qtmp.q1*q2 + qtmp.q2*q1; 374 | return vtmp; 375 | } 376 | 377 | void CQuat::SetYaw(double yaw) 378 | { 379 | CVect3 att = q2att(*this); 380 | att.k = yaw; 381 | *this = a2qua(att); 382 | } 383 | 384 | void normlize(CQuat *q) 385 | { 386 | double nq=sqrt(q->q0*q->q0+q->q1*q->q1+q->q2*q->q2+q->q3*q->q3); 387 | q->q0 /= nq, q->q1 /= nq, q->q2 /= nq, q->q3 /= nq; 388 | } 389 | 390 | CVect3 q2rv(const CQuat &q) 391 | { 392 | CQuat dq; 393 | dq = q; 394 | if(dq.q0<0) { dq.q0=-dq.q0, dq.q1=-dq.q1, dq.q2=-dq.q2, dq.q3=-dq.q3; } 395 | if(dq.q0>1.0) dq.q0=1.0; 396 | double n2 = acos(dq.q0), f; 397 | if(n2>1.0e-20) 398 | { 399 | f = 2.0/(sin(n2)/n2); 400 | } 401 | else 402 | { 403 | f = 2.0; 404 | } 405 | return CVect3(dq.q1,dq.q2,dq.q3)*f; 406 | } 407 | 408 | CQuat UpDown(const CQuat &q) 409 | { 410 | CVect3 att = q2att(q); 411 | att.i = -att.i; att.j += PI; 412 | return a2qua(att); 413 | } 414 | 415 | /*************************** class CMat3 *********************************/ 416 | CMat3::CMat3(void) 417 | { 418 | } 419 | 420 | CMat3::CMat3(double xx, double xy, double xz, 421 | double yx, double yy, double yz, 422 | double zx, double zy, double zz ) 423 | { 424 | e00=xx,e01=xy,e02=xz; e10=yx,e11=yy,e12=yz; e20=zx,e21=zy,e22=zz; 425 | } 426 | 427 | CMat3::CMat3(const CVect3 &v0, const CVect3 &v1, const CVect3 &v2) 428 | { 429 | e00 = v0.i, e01 = v0.j, e02 = v0.k; 430 | e10 = v1.i, e11 = v1.j, e12 = v1.k; 431 | e20 = v2.i, e21 = v2.j, e22 = v2.k; 432 | } 433 | 434 | CMat3 dv2att(CVect3 &va1, const CVect3 &va2, CVect3 &vb1, const CVect3 &vb2) 435 | { 436 | CVect3 a=va1*va2, b=vb1*vb2, aa=a*va1, bb=b*vb1; 437 | CMat3 Ma(va1/norm(va1),a/norm(a),aa/norm(aa)), Mb(vb1/norm(vb1),b/norm(b),bb/norm(bb)); 438 | return (~Ma)*(Mb); //Cab 439 | } 440 | 441 | CMat3 operator-(const CMat3 &m) 442 | { 443 | return CMat3(-m.e00,-m.e01,-m.e02,-m.e10,-m.e11,-m.e12,-m.e20,-m.e21,-m.e22); 444 | } 445 | 446 | CMat3 operator~(const CMat3 &m) 447 | { 448 | return CMat3(m.e00,m.e10,m.e20, m.e01,m.e11,m.e21, m.e02,m.e12,m.e22); 449 | } 450 | 451 | CMat3 CMat3::operator*(const CMat3 &mat) const 452 | { 453 | CMat3 mtmp; 454 | mtmp.e00 = e00*mat.e00 + e01*mat.e10 + e02*mat.e20; 455 | mtmp.e01 = e00*mat.e01 + e01*mat.e11 + e02*mat.e21; 456 | mtmp.e02 = e00*mat.e02 + e01*mat.e12 + e02*mat.e22; 457 | mtmp.e10 = e10*mat.e00 + e11*mat.e10 + e12*mat.e20; 458 | mtmp.e11 = e10*mat.e01 + e11*mat.e11 + e12*mat.e21; 459 | mtmp.e12 = e10*mat.e02 + e11*mat.e12 + e12*mat.e22; 460 | mtmp.e20 = e20*mat.e00 + e21*mat.e10 + e22*mat.e20; 461 | mtmp.e21 = e20*mat.e01 + e21*mat.e11 + e22*mat.e21; 462 | mtmp.e22 = e20*mat.e02 + e21*mat.e12 + e22*mat.e22; 463 | return mtmp; 464 | } 465 | 466 | CMat3 CMat3::operator+(const CMat3 &mat) const 467 | { 468 | CMat3 mtmp; 469 | mtmp.e00 = e00 + mat.e00; mtmp.e01 = e01 + mat.e01; mtmp.e02 = e02 + mat.e02; 470 | mtmp.e10 = e10 + mat.e10; mtmp.e11 = e11 + mat.e11; mtmp.e12 = e12 + mat.e12; 471 | mtmp.e20 = e20 + mat.e20; mtmp.e21 = e21 + mat.e21; mtmp.e22 = e22 + mat.e22; 472 | return mtmp; 473 | } 474 | 475 | CMat3 CMat3::operator-(const CMat3 &mat) const 476 | { 477 | CMat3 mtmp; 478 | mtmp.e00 = e00 - mat.e00; mtmp.e01 = e01 - mat.e01; mtmp.e02 = e02 - mat.e02; 479 | mtmp.e10 = e10 - mat.e10; mtmp.e11 = e11 - mat.e11; mtmp.e12 = e12 - mat.e12; 480 | mtmp.e20 = e20 - mat.e20; mtmp.e21 = e21 - mat.e21; mtmp.e22 = e22 - mat.e22; 481 | return mtmp; 482 | } 483 | 484 | CMat3 CMat3::operator*(double f) const 485 | { 486 | return CMat3(e00*f,e01*f,e02*f, e10*f,e11*f,e12*f, e21*f,e20*f,e22*f); 487 | } 488 | 489 | CMat3 operator*(double f, const CMat3 &m) 490 | { 491 | 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); 492 | } 493 | 494 | CVect3 CMat3::operator*(const CVect3 &v) const 495 | { 496 | 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); 497 | } 498 | 499 | double det(const CMat3 &m) 500 | { 501 | 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); 502 | } 503 | 504 | CQuat a2qua(double pitch, double roll, double yaw) 505 | { 506 | pitch /= 2.0, roll /= 2.0, yaw /= 2.0; 507 | double sp = sin(pitch), sr = sin(roll), sy = sin(yaw), 508 | cp = cos(pitch), cr = cos(roll), cy = cos(yaw); 509 | CQuat qnb; 510 | qnb.q0 = cp*cr*cy - sp*sr*sy; 511 | qnb.q1 = sp*cr*cy - cp*sr*sy; 512 | qnb.q2 = cp*sr*cy + sp*cr*sy; 513 | qnb.q3 = cp*cr*sy + sp*sr*cy; 514 | return qnb; 515 | } 516 | 517 | CQuat a2qua(const CVect3 &att) 518 | { 519 | return a2qua(att.i, att.j, att.k); 520 | } 521 | 522 | CMat3 a2mat(const CVect3 &att) 523 | { 524 | double si = sin(att.i), ci = cos(att.i), 525 | sj = sin(att.j), cj = cos(att.j), 526 | sk = sin(att.k), ck = cos(att.k); 527 | CMat3 Cnb; 528 | Cnb.e00 = cj*ck - si*sj*sk; Cnb.e01 = -ci*sk; Cnb.e02 = sj*ck + si*cj*sk; 529 | Cnb.e10 = cj*sk + si*sj*ck; Cnb.e11 = ci*ck; Cnb.e12 = sj*sk - si*cj*ck; 530 | Cnb.e20 = -ci*sj; Cnb.e21 = si; Cnb.e22 = ci*cj; 531 | return Cnb; 532 | } 533 | 534 | CVect3 m2att(const CMat3 &Cnb) 535 | { 536 | CVect3 att; 537 | att.i = asinEx(Cnb.e21); 538 | att.j = atan2Ex(-Cnb.e20, Cnb.e22); 539 | att.k = atan2Ex(-Cnb.e01, Cnb.e11); 540 | return att; 541 | } 542 | 543 | CQuat m2qua(const CMat3 &Cnb) 544 | { 545 | double q0, q1, q2, q3, qq4; 546 | if(Cnb.e00>=Cnb.e11+Cnb.e22) 547 | { 548 | q1 = 0.5*sqrt(1+Cnb.e00-Cnb.e11-Cnb.e22); qq4 = 4*q1; 549 | q0 = (Cnb.e21-Cnb.e12)/qq4; q2 = (Cnb.e01+Cnb.e10)/qq4; q3 = (Cnb.e02+Cnb.e20)/qq4; 550 | } 551 | else if(Cnb.e11>=Cnb.e00+Cnb.e22) 552 | { 553 | q2 = 0.5*sqrt(1-Cnb.e00+Cnb.e11-Cnb.e22); qq4 = 4*q2; 554 | q0 = (Cnb.e02-Cnb.e20)/qq4; q1 = (Cnb.e01+Cnb.e10)/qq4; q3 = (Cnb.e12+Cnb.e21)/qq4; 555 | } 556 | else if(Cnb.e22>=Cnb.e00+Cnb.e11) 557 | { 558 | q3 = 0.5*sqrt(1-Cnb.e00-Cnb.e11+Cnb.e22); qq4 = 4*q3; 559 | q0 = (Cnb.e10-Cnb.e01)/qq4; q1 = (Cnb.e02+Cnb.e20)/qq4; q2 = (Cnb.e12+Cnb.e21)/qq4; 560 | } 561 | else 562 | { 563 | q0 = 0.5*sqrt(1+Cnb.e00+Cnb.e11+Cnb.e22); qq4 = 4*q0; 564 | q1 = (Cnb.e21-Cnb.e12)/qq4; q2 = (Cnb.e02-Cnb.e20)/qq4; q3 = (Cnb.e10-Cnb.e01)/qq4; 565 | } 566 | double nq = sqrt(q0*q0+q1*q1+q2*q2+q3*q3); 567 | q0 /= nq; q1 /= nq; q2 /= nq; q3 /= nq; 568 | return CQuat(q0, q1, q2, q3); 569 | } 570 | 571 | CVect3 q2att(const CQuat &qnb) 572 | { 573 | double q11 = qnb.q0*qnb.q0, q12 = qnb.q0*qnb.q1, q13 = qnb.q0*qnb.q2, q14 = qnb.q0*qnb.q3, 574 | q22 = qnb.q1*qnb.q1, q23 = qnb.q1*qnb.q2, q24 = qnb.q1*qnb.q3, 575 | q33 = qnb.q2*qnb.q2, q34 = qnb.q2*qnb.q3, 576 | q44 = qnb.q3*qnb.q3; 577 | CVect3 att; 578 | att.i = asinEx(2*(q34+q12)); 579 | att.j = atan2Ex(-2*(q24-q13), q11-q22-q33+q44); 580 | att.k = atan2Ex(-2*(q23-q14), q11-q22+q33-q44); 581 | return att; 582 | } 583 | 584 | CMat3 q2mat(const CQuat &qnb) 585 | { 586 | double q11 = qnb.q0*qnb.q0, q12 = qnb.q0*qnb.q1, q13 = qnb.q0*qnb.q2, q14 = qnb.q0*qnb.q3, 587 | q22 = qnb.q1*qnb.q1, q23 = qnb.q1*qnb.q2, q24 = qnb.q1*qnb.q3, 588 | q33 = qnb.q2*qnb.q2, q34 = qnb.q2*qnb.q3, 589 | q44 = qnb.q3*qnb.q3; 590 | CMat3 Cnb; 591 | Cnb.e00 = q11+q22-q33-q44, Cnb.e01 = 2*(q23-q14), Cnb.e02 = 2*(q24+q13), 592 | Cnb.e10 = 2*(q23+q14), Cnb.e11 = q11-q22+q33-q44, Cnb.e12 = 2*(q34-q12), 593 | Cnb.e20 = 2*(q24-q13), Cnb.e21 = 2*(q34+q12), Cnb.e22 = q11-q22-q33+q44; 594 | return Cnb; 595 | } 596 | 597 | CMat3 inv(const CMat3 &m) 598 | { 599 | double nm; 600 | nm = 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); 601 | CMat3 mtmp; 602 | mtmp.e00 = (m.e11*m.e22-m.e12*m.e21)/nm; 603 | mtmp.e10 = -(m.e10*m.e22-m.e12*m.e20)/nm; 604 | mtmp.e20 = (m.e10*m.e21-m.e11*m.e20)/nm; 605 | mtmp.e01 = -(m.e01*m.e22-m.e02*m.e21)/nm; 606 | mtmp.e11 = (m.e00*m.e22-m.e02*m.e20)/nm; 607 | mtmp.e21 = -(m.e00*m.e21-m.e01*m.e20)/nm; 608 | mtmp.e02 = (m.e01*m.e12-m.e02*m.e11)/nm; 609 | mtmp.e12 = -(m.e00*m.e12-m.e02*m.e10)/nm; 610 | mtmp.e22 = (m.e00*m.e11-m.e01*m.e10)/nm; 611 | return mtmp; 612 | } 613 | 614 | CVect3 diag(const CMat3 &m) 615 | { 616 | return CVect3(m.e00, m.e11, m.e22); 617 | } 618 | 619 | CMat3 diag(const CVect3 &v) 620 | { 621 | return CMat3(v.i,0,0, 0,v.j,0, 0,0,v.k); 622 | } 623 | 624 | /*************************** class CMat *********************************/ 625 | CMat::CMat(void) 626 | { 627 | #ifdef MAT_COUNT_STATISTIC 628 | #pragma message(" MAT_COUNT_STATISTIC") 629 | if(iMax<++iCount) iMax = iCount; 630 | #endif 631 | } 632 | 633 | CMat::CMat(int row0, int clm0) 634 | { 635 | #ifdef MAT_COUNT_STATISTIC 636 | if(iMax<++iCount) iMax = iCount; 637 | #endif 638 | row=row0; clm=clm0; rc=row*clm; 639 | } 640 | 641 | CMat::CMat(int row0, int clm0, double f) 642 | { 643 | #ifdef MAT_COUNT_STATISTIC 644 | if(iMax<++iCount) iMax = iCount; 645 | #endif 646 | row=row0; clm=clm0; rc=row*clm; 647 | for(double *pd=dd, *pEnd=&dd[rc]; pdclm==m0.row); 673 | CMat mtmp(this->row,m0.clm); 674 | int m=this->row, k=this->clm, n=m0.clm; 675 | double *p=mtmp.dd; const double *p1i=this->dd, *p2=m0.dd; 676 | for(int i=0; iclm==v.row); 692 | CVect vtmp(this->row); 693 | double *p=vtmp.dd, *pEnd=&vtmp.dd[vtmp.row]; const double *p1ij=this->dd, *p2End=&v.dd[v.row]; 694 | for(; pdd, *p2=m0.dd; 711 | while(pdd, *p2=m0.dd; 733 | while(pdd; 745 | while(pdd[r*this->clm+c]; 808 | } 809 | 810 | void CMat::SetRow(int i, double f, ...) 811 | { 812 | va_list vl; 813 | va_start(vl, f); 814 | for(double *p=&dd[i*clm], *pEnd=p+clm; prow, this->clm, 0.0); 926 | va_list vl; 927 | va_start(vl, f); 928 | double *p=dd, *pEnd=&dd[rc]; 929 | for(int row1=row+1; prow, this->clm, 0.0); 937 | va_list vl; 938 | va_start(vl, f); 939 | double *p=dd, *pEnd=&dd[rc]; 940 | for(int row1=row+1; p0.0) n1 += *p; 951 | else n1 -= *p; 952 | } 953 | return n1; 954 | } 955 | 956 | CVect diag(const CMat &m) 957 | { 958 | int row1 = m.row+1; 959 | CVect vtmp(m.row,1); 960 | double *p=vtmp.dd, *pEnd=&vtmp.dd[vtmp.row]; 961 | for(const double *p1=m.dd; pv.clm ? v.row : v.clm, rc1=rc+1; 997 | CMat mtmp(rc,rc,0.0); 998 | double *p=mtmp.dd; 999 | for(const double *p1=v.dd, *p1End=&v.dd[rc]; p1clm?row:clm; i0 ? *p : -*p; } 1189 | return res; 1190 | } 1191 | 1192 | double norm(const CVect &v) 1193 | { 1194 | const double *p=v.dd, *pEnd=&v.dd[v.rc]; 1195 | double f=0.0; 1196 | for(; pdd[r]; 1203 | } 1204 | 1205 | void CVect::Set(double f, ...) 1206 | { 1207 | psinsassert(rc<=MMD); 1208 | va_list vl; 1209 | va_start(vl, f); 1210 | for(int i=0; i0; i--) 1243 | { 1244 | x[i] = x[i-1]; y[i] = y[i-1]; 1245 | y0 += b[i]*x[i] - a[i]*y[i]; 1246 | } 1247 | x[0] = x0; 1248 | y0 += b[0]*x0; 1249 | y[0] = y0; 1250 | return y0; 1251 | } 1252 | 1253 | /*************************** class CV3IIR *********************************/ 1254 | CIIRV3::CIIRV3(void) 1255 | { 1256 | } 1257 | 1258 | CIIRV3::CIIRV3(double *b0, double *a0, int n0, double *b1, double *a1, int n1, 1259 | double *b2, double *a2, int n2) 1260 | { 1261 | iir0 = CIIR(b0, a0, n0); 1262 | if(n1==0) iir1 = iir0; // iir1 the same as iir0 1263 | else iir1 = CIIR(b1, a1, n1); 1264 | if(n2==0) iir2 = iir0; // iir2 the same as iir0 1265 | else iir2 = CIIR(b2, a2, n2); 1266 | } 1267 | 1268 | CVect3 CIIRV3::Update(CVect3 &x) 1269 | { 1270 | return y = CVect3(iir0.Update(x.i), iir1.Update(x.j), iir2.Update(x.k)); 1271 | } 1272 | 1273 | /*************************** class CRAvar *********************************/ 1274 | CRAvar::CRAvar() 1275 | { 1276 | } 1277 | 1278 | CRAvar::CRAvar(int nR0, int maxCount0) 1279 | { 1280 | psinsassert(nR0nR0 = nR0; 1282 | for(int i=0; iR0[i] = r0*r0; 1288 | this->tau[i] = tau; 1289 | this->r0[i] = 0.0; Rmaxflag[i] = Rmaxcount[i]; 1290 | this->Rmax[i] = rmax==0.0 ? 100.0*this->R0[i] : rmax*rmax; 1291 | this->Rmin[i] = rmin==0.0 ? 0.01*this->R0[i] : rmin*rmin; 1292 | } 1293 | 1294 | void CRAvar::set(const CVect3 &r0, const CVect3 &tau, const CVect3 &rmax, const CVect3 &rmin) 1295 | { 1296 | const double *pr0=&r0.i, *ptau=&tau.i, *prmax=&rmax.i, *prmin=&rmin.i; 1297 | for(int i=0; i<3; i++,pr0++,ptau++,prmax++,prmin++) 1298 | set(*pr0, *ptau, *prmax, *prmin, i); 1299 | } 1300 | 1301 | void CRAvar::set(const CVect &r0, const CVect &tau, const CVect &rmax, const CVect &rmin) 1302 | { 1303 | const double *pr0=r0.dd, *ptau=tau.dd, *prmax=rmax.dd, *prmin=rmin.dd; 1304 | for(int i=0; iINF/2) return; 1311 | double tstau = ts>tau[i] ? 1.0 : ts/tau[i]; 1312 | double dr2=r-r0[i]; dr2=dr2*dr2; r0[i]=r; 1313 | if(dr2>R0[i]) R0[i]=dr2; else R0[i]=(1.0-tstau)*R0[i]+tstau*dr2; 1314 | if(R0[i]Rmax[i]) {R0[i]=Rmax[i];Rmaxflag[i]=Rmaxcount[i];} else {Rmaxflag[i]-=Rmaxflag[i]>0;} 1316 | } 1317 | 1318 | void CRAvar::Update(const CVect3 &r, double ts) 1319 | { 1320 | const double *pr=&r.i; 1321 | for(int i=0; i<3; i++,pr++) 1322 | Update(*pr, ts, i); 1323 | } 1324 | 1325 | void CRAvar::Update(const CVect &r, double ts) 1326 | { 1327 | const double *pr=r.dd; 1328 | for(int i=0; i row) rowcnt = row; 1396 | int idxnext = (idxpush >= row - 1) ? 0 : idxpush + 1; 1397 | for (int i = 0; i < clm; i++) 1398 | { 1399 | double f=*pf++; if(f>1e5) f=1e5; else if(f<-1e5) f=-1e5; 1400 | pData[i][idxpush] = f; 1401 | Sx[i] += f - pData[i][idxnext]; 1402 | Sx2[i] += f*f - pData[i][idxnext] * pData[i][idxnext]; 1403 | mx[i] = Sx[i] / rowcnt; 1404 | stdx[i] = sqrt(Sx2[i] / rowcnt - mx[i] * mx[i]) * stdsf; // Dx = E(x^2) - (Ex)^2 1405 | } 1406 | if (++idxpush == row) { 1407 | idxpush = 0; 1408 | } 1409 | return idxpush == 0; 1410 | } 1411 | 1412 | BOOL CVARn::Update(double f, ...) 1413 | { 1414 | va_list vl; 1415 | va_start(vl, f); 1416 | for (int i = 0; i < clm; i++) 1417 | { 1418 | pd[i] = f; 1419 | f = va_arg(vl, double); 1420 | } 1421 | va_end(vl); 1422 | return Update(pd); 1423 | } 1424 | 1425 | /*************************** class CKalman *********************************/ 1426 | CKalman::CKalman(int nq0, int nr0) 1427 | { 1428 | psinsassert(nq0<=MMD&&nr0<=MMD); 1429 | kftk = 0.0; 1430 | nq = nq0; nr = nr0; 1431 | Ft = Pk = CMat(nq,nq,0.0); 1432 | Hk = CMat(nr,nq,0.0); Fading = CMat(nr,nq,1.0); zfdafa = 0.1; 1433 | Qt = Pmin = Xk = CVect(nq,0.0); Xmax = Pmax = CVect(nq,INF); 1434 | Zk = CVect(nr,0.0); Rt = CVect(nr,INF); rts = CVect(nr,1.0); Zfd = CVect(nr,0.0); Zfd0 = CVect(nr,INF); 1435 | Rmax = CVect(nr,INF); Rmin = Rb = CVect(nr,0.0); Rbeta = CVect(nr,1.0); 1436 | for(int i=0; iEPS) 1479 | RAdaptive(i, r, Pz0); 1480 | if(Zfd.dd[i]1.0) Pk *= fading; 1489 | XPConstrain(); 1490 | symmetry(Pk); 1491 | SetMeasFlag(0); 1492 | return measflag; 1493 | } 1494 | 1495 | int CKalman::RAdaptive(int i, double r, double Pr) 1496 | { 1497 | double rr=r*r-Pr; 1498 | if(rrRmax.dd[i]) Rt.dd[i] = Rmax.dd[i]; 1500 | // else Rt.dd[i] = (1.0-Rbeta.dd[i])*Rt.dd[i]+Rbeta.dd[i]*rr; 1501 | if(rr>Rmax.dd[i]) { Rt.dd[i]=Rmax.dd[i]; Rmaxcount[i]++; } 1502 | else { Rt.dd[i]=(1.0-Rbeta.dd[i])*Rt.dd[i]+Rbeta.dd[i]*rr; Rmaxcount[i]=0; } 1503 | Rbeta.dd[i] = Rbeta.dd[i]/(Rbeta.dd[i]+Rb.dd[i]); // beta = beta / (beta+b) 1504 | int adptOK = (Rmaxcount[i]==0||Rmaxcount[i]>Rmaxcount0[i]) ? 1: 0; 1505 | return adptOK; 1506 | } 1507 | 1508 | void CKalman::RPkFading(int i) 1509 | { 1510 | Zfd.dd[i] = Zfd.dd[i]*(1-zfdafa) + Zk.dd[i]*zfdafa; 1511 | if(Zfd.dd[i]>Zfd0.dd[i] || Zfd.dd[i]<-Zfd0.dd[i]) 1512 | DVMDVafa(Fading.GetRow(i), Pk); 1513 | } 1514 | 1515 | void CKalman::XPConstrain(void) 1516 | { 1517 | int i=0, nq1=nq+1; 1518 | for(double *px=Xk.dd,*pxmax=Xmax.dd,*p=Pk.dd,*pmin=Pmin.dd,*pminEnd=&Pmin.dd[nq],*pmax=Pmax.dd; 1519 | pmin*pxmax) // Xk constrain 1522 | { 1523 | *px = *pxmax; 1524 | } 1525 | else if(*px<-*pxmax) 1526 | { 1527 | *px = -*pxmax; 1528 | } 1529 | if(*p<*pmin) // Pk constrain 1530 | { 1531 | *p = *pmin; 1532 | } 1533 | else if(*p>*pmax) 1534 | { 1535 | double sqf=sqrt(*pmax/(*p))*0.95; 1536 | for(double *prow=&Pk.dd[i*Pk.clm],*prowEnd=prow+nq,*pclm=&Pk.dd[i]; prow*pMax) *pXk = *pMax-*pTotal; 1558 | else if(*pTotal+*pXk<-*pMax) *pXk = -*pMax-*pTotal; 1559 | } 1560 | *p -= *pXk; 1561 | *pTotal += *pXk; 1562 | } 1563 | else 1564 | { 1565 | *pXk = 0.0; 1566 | } 1567 | } 1568 | } 1569 | 1570 | /*************************** class CSINSKF *********************************/ 1571 | CSINSKF::CSINSKF(int nq0, int nr0):CKalman(nq0,nr0) 1572 | { 1573 | sins = CSINS(qI, O31, O31); 1574 | SetFt(); 1575 | SetHk(); 1576 | } 1577 | 1578 | void CSINSKF::Init(const CSINS &sins0, int grade) 1579 | { 1580 | sins = sins0; 1581 | sins.Rwfa.set( 1582 | 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), 1583 | CVect(9, 1.0,1.0,1.0, 1.0,1.0,1.0, 1.0,1.0,1.0), 1584 | 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), 1585 | 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) 1586 | ); 1587 | // a example for 15-state(phi,dvn,dpos,eb,db) KF setting 1588 | if(grade==0) // inertial-grade 1589 | { 1590 | 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, 1591 | 10.0*glv.dph,10.0*glv.dph,10.0*glv.dph, 10.0*glv.mg,10.0*glv.mg,10.0*glv.mg); 1592 | 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, 1593 | 0.001*glv.dph,0.001*glv.dph,0.001*glv.dph, 1.0*glv.ug,1.0*glv.ug,5.0*glv.ug); 1594 | 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, 1595 | 0.1*glv.dph,0.1*glv.dph,0.1*glv.dph, 1.0*glv.mg,1.0*glv.mg,1.0*glv.mg); 1596 | 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, 1597 | 0.0*glv.dphpsh,0.0*glv.dphpsh,0.0*glv.dphpsh, 0.0*glv.ugpsh,0.0*glv.ugpsh,0.0*glv.ugpsh); 1598 | 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); 1599 | Rt.Set2(0.2,0.2,0.6, 10.0/glv.Re,10.0/glv.Re, 30.0); 1600 | Rmax = Rt*10000; Rmin = Rt*0.01; Rb = 0.9; 1601 | 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); 1602 | } 1603 | else if(grade==1) // MEMS-grade 1604 | { 1605 | 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, 1606 | 5000.0*glv.dph,5000.0*glv.dph,5000.0*glv.dph, 50.0*glv.mg,50.0*glv.mg,50.0*glv.mg); 1607 | 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, 1608 | 1.0*glv.dph,1.0*glv.dph,1.0*glv.dph, 50.0*glv.ug,50.0*glv.ug,50.0*glv.ug); 1609 | 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, 1610 | 100.0*glv.dph,101.0*glv.dph,102.0*glv.dph, 1.0*glv.mg,1.01*glv.mg,10.0*glv.mg); 1611 | 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, 1612 | 0.0*glv.dphpsh,0.0*glv.dphpsh,0.0*glv.dphpsh, 0.0*glv.ugpsh,0.0*glv.ugpsh,0.0*glv.ugpsh); 1613 | 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); 1614 | Rt.Set2(0.2,0.2,0.6, 10.0/glv.Re,10.0/glv.Re,30.0); 1615 | Rmax = Rt*10000; Rmin = Rt*0.01; Rb = 0.9; 1616 | 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); 1617 | } 1618 | Zfd0.Set(1.0,15.0,1.0, 100.0/RE,100.0/RE,100.0, INF); zfdafa = 0.1; 1619 | Fading(0,1)=1.01; Fading(0,3)=1.01; Fading(0,7)=1.01; 1620 | Fading(1,0)=1.01; Fading(1,4)=1.01; Fading(1,6)=1.01; 1621 | Fading(2,5)=1.01; Fading(2,8)=1.01; Fading(2,14)=1.01; 1622 | Fading(3,0)=1.01; Fading(3,4)=1.01; Fading(3,6)=1.01; 1623 | Fading(4,1)=1.01; Fading(4,3)=1.01; Fading(4,7)=1.01; 1624 | Fading(5,5)=1.01; Fading(5,8)=1.01; Fading(5,14)=1.01; 1625 | } 1626 | 1627 | void CSINSKF::SetFt(void) 1628 | { 1629 | sins.etm(); 1630 | // Ft = [ Maa Mav Map -Cnb O33 1631 | // Mva Mvv Mvp O33 Cnb 1632 | // O33 Mpv Mpp O33 O33 1633 | // zeros(6,9) diag(-1./[ins.tauG;ins.tauA]) ]; 1634 | Ft.SetMat3(0,0,sins.Maa), Ft.SetMat3(0,3,sins.Mav), Ft.SetMat3(0,6,sins.Map), Ft.SetMat3(0,9,-sins.Cnb); 1635 | Ft.SetMat3(3,0,sins.Mva), Ft.SetMat3(3,3,sins.Mvv), Ft.SetMat3(3,6,sins.Mvp), Ft.SetMat3(3,12,sins.Cnb); 1636 | Ft.SetMat3(6,3,sins.Mpv), Ft.SetMat3(6,6,sins.Mpp); 1637 | Ft.SetDiagVect3( 9, 9, sins._betaGyro); 1638 | Ft.SetDiagVect3(12,12, sins._betaAcc); 1639 | } 1640 | 1641 | void CSINSKF::SetHk(void) 1642 | { 1643 | // an example for SINS/GPS vn&pos measurement 1644 | Hk(0,3) = Hk(1,4) = Hk(2,5) = 1.0; 1645 | Hk(3,6) = Hk(4,7) = Hk(5,8) = 1.0; 1646 | } 1647 | 1648 | int CSINSKF::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts) 1649 | { 1650 | sins.Update(pwm, pvm, nSamples, ts); 1651 | TimeUpdate(sins.nts); kftk = sins.tk; 1652 | return MeasUpdate(); 1653 | } 1654 | 1655 | void CSINSKF::Feedback(double fbts) 1656 | { 1657 | CKalman::Feedback(fbts); 1658 | sins.qnb -= *(CVect3*)&FBXk.dd[0]; sins.vn -= *(CVect3*)&FBXk.dd[ 3]; sins.pos -= *(CVect3*)&FBXk.dd[6]; 1659 | sins.eb += *(CVect3*)&FBXk.dd[9]; sins.db += *(CVect3*)&FBXk.dd[12]; 1660 | } 1661 | 1662 | void CSINSKF::QtMarkovGA(const CVect3 &tauG, const CVect3 &sRG, const CVect3 &tauA=I31, const CVect3 &sRA=O31) 1663 | { 1664 | sins.SetTauGA(tauG, tauA); 1665 | *(CVect3*)&Qt.dd[ 9] = MKQt(sRG, sins.tauGyro); 1666 | *(CVect3*)&Qt.dd[12] = MKQt(sRA, sins.tauAcc); 1667 | } 1668 | 1669 | /*************************** class CSINSTDKF *********************************/ 1670 | CSINSTDKF::CSINSTDKF(int nq0, int nr0):CSINSKF(nq0,nr0) 1671 | { 1672 | Fk = Pk1 = CMat(nq,nq, 0.0); 1673 | Pxz = Qk = Kk = tmeas = CVect(nr, 0.0); 1674 | tdts = 0.0; 1675 | maxStep = 2*(nq+nr)+3; 1676 | TDReset(); 1677 | } 1678 | 1679 | void CSINSTDKF::TDReset(void) 1680 | { 1681 | iter = -2; 1682 | ifn = 0; meanfn = O31; 1683 | SetMeasFlag(0); 1684 | } 1685 | 1686 | int CSINSTDKF::TDUpdate(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts, int nStep) 1687 | { 1688 | // if(sins.tk>99) 1689 | // int debugi = 1; 1690 | 1691 | sins.Update(pwm, pvm, nSamples, ts); 1692 | Feedback(sins.nts); 1693 | 1694 | measRes = 0; 1695 | 1696 | if(nStep<=0||nStep>=maxStep) { nStep=maxStep; } 1697 | tdStep = nStep; 1698 | 1699 | tdts += sins.nts; kftk = sins.tk; 1700 | meanfn = meanfn+sins.fn; ifn++; 1701 | for(int i=0; i (nq-1): Fk*Pk 1718 | { 1719 | int row=iter; 1720 | RowMul(Pk1, Fk, Pk, row); 1721 | } 1722 | else if(iter<2*nq) // nq -> (2*nq-1): Fk*Pk*Fk+Qk 1723 | { 1724 | int row=iter-nq; 1725 | RowMulT(Pk, Pk1, Fk, row); 1726 | Pk.dd[nq*row+row] += Qk.dd[row]; 1727 | // if(row==nq-1) { Pk += Qk; } 1728 | } 1729 | else if(iter<2*(nq+nr)) // (2*nq) -> (2*(nq+nr)-1): sequential measurement updating 1730 | { 1731 | int row=(iter-2*Ft.row)/2; 1732 | int flag = measflag&(0x01<EPS) 1743 | adptOKi = RAdaptive(row, innovation, Pz0); 1744 | double Pzz = Pz0 + Rt(row)/rts(row); 1745 | Kk = Pxz*(1.0/Pzz); 1746 | } 1747 | else 1748 | { 1749 | measflag ^= flag; 1750 | if(adptOKi) 1751 | { 1752 | measRes |= flag; 1753 | Xk += Kk*innovation; 1754 | Pk -= Kk*(~Pxz); 1755 | } 1756 | if(Zfd0.dd[row]=2*(nq+nr)+1) // 2*(nq+nr)+1: Miscellanous 1773 | { 1774 | Miscellanous(); 1775 | iter = -3; 1776 | } 1777 | iter++; 1778 | } 1779 | 1780 | return measRes; 1781 | } 1782 | 1783 | /*************************** class CSINSGPS *********************************/ 1784 | CSINSGPSOD::CSINSGPSOD(void):CSINSTDKF(15, 10) 1785 | { 1786 | vbINS = vbOD = O31; tODInt = 0.0; 1787 | Cbo = I33; 1788 | Hk(9,2) = 1.0; 1789 | measGPSvnValid = measGPSposValid = measODvbValid = measMAGyawValid = 0; 1790 | } 1791 | 1792 | #ifndef CSINSGPSOD_INIT_BY_USER 1793 | // may be copied & implemented by user 1794 | void CSINSGPSOD::Init(const CSINS &sins0, int grade) 1795 | { 1796 | CSINSKF::Init(sins0, grade); 1797 | 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, 1798 | 0.0*glv.dphpsh,0.0*glv.dphpsh,0.0*glv.dphpsh, 0.0*glv.ugpsh,0.0*glv.ugpsh,0.0*glv.ugpsh); 1799 | Rt.Set2(0.2,0.2,0.6, 10.0/glv.Re,10.0/glv.Re,30.0, 0.1,1000.0,0.1, 1.0*glv.deg); 1800 | Rmax = Rt*10000; Rmin = Rt*0.01; Rb = 0.9; 1801 | } 1802 | #else 1803 | #pragma message(" CSINSGPSOD_INIT_BY_USER") 1804 | #endif 1805 | 1806 | void CSINSGPSOD::SetMeas(void) 1807 | { 1808 | if(measGPSposValid) 1809 | { 1810 | SetMeasFlag(0070); 1811 | measGPSposValid = 0; 1812 | } 1813 | if(measGPSvnValid) 1814 | { 1815 | SetMeasFlag(0007); 1816 | measGPSvnValid = 0; 1817 | } 1818 | if(measODvbValid) 1819 | { 1820 | SetMeasFlag(0700); 1821 | measODvbValid = 0; 1822 | } 1823 | if(measMAGyawValid) 1824 | { 1825 | SetMeasFlag(01000); 1826 | measMAGyawValid = 0; 1827 | } 1828 | } 1829 | 1830 | void CSINSGPSOD::SetMeasGPS(const CVect3 &pgps, const CVect3 &vgps) 1831 | { 1832 | if(!IsZero(pgps)) 1833 | { 1834 | *(CVect3*)&Zk.dd[3] = sins.pos - pgps; 1835 | measGPSposValid = 1; 1836 | } 1837 | if(!IsZero(vgps)) 1838 | { 1839 | *(CVect3*)&Zk.dd[0] = sins.vn - vgps; 1840 | measGPSvnValid = 1; 1841 | } 1842 | } 1843 | 1844 | void CSINSGPSOD::SetMeasOD(double dSod, double ts) 1845 | { 1846 | if(sins.wnb.k<5.0*glv.dps) 1847 | { 1848 | tODInt += ts; 1849 | vbINS += sins.vb; 1850 | vbOD += Cbo*CVect3(0,dSod/ts,0); 1851 | if(tODInt>1.0) 1852 | { 1853 | *(CVect3*)&Zk.dd[6] = vbINS - vbOD; 1854 | Hk.SetMat3(6, 3, sins.Cbn); 1855 | vbINS = vbOD = O31; tODInt = 0.0; 1856 | measODvbValid = 1; 1857 | } 1858 | } 1859 | else 1860 | { 1861 | vbINS = vbOD = O31; tODInt = 0.0; 1862 | } 1863 | } 1864 | 1865 | void CSINSGPSOD::SetMeasYaw(double ymag) 1866 | { 1867 | if(ymag>EPS || ymag<-EPS) // !IsZero(yawGPS) 1868 | if(sins.att.i<30*DEG && sins.att.i>-30*DEG) 1869 | { 1870 | *(CVect3*)&Zk.dd[9] = -diffYaw(sins.att.k, ymag); 1871 | measMAGyawValid = 1; 1872 | } 1873 | } 1874 | 1875 | /*************************** class CEarth *********************************/ 1876 | CEarth::CEarth(double a0, double f0, double g0) 1877 | { 1878 | a = a0; f = f0; wie = glv.wie; 1879 | b = (1-f)*a; 1880 | e = sqrt(a*a-b*b)/a; e2 = e*e; 1881 | gn = O31; pgn = 0; 1882 | Update(O31); 1883 | } 1884 | 1885 | void CEarth::Update(const CVect3 &pos, const CVect3 &vn) 1886 | { 1887 | #ifdef PSINS_LOW_GRADE_MEMS 1888 | this->pos = pos; this->vn = vn; 1889 | sl = sin(pos.i), cl = cos(pos.i), tl = sl/cl; 1890 | double sq = 1-e2*sl*sl, sq2 = sqrt(sq); 1891 | RMh = a*(1-e2)/sq/sq2+pos.k; f_RMh = 1.0/RMh; 1892 | RNh = a/sq2+pos.k; clRNh = cl*RNh; f_RNh = 1.0/RNh; f_clRNh = 1.0/clRNh; 1893 | // wnie.i = 0.0, wnie.j = wie*cl, wnie.k = wie*sl; 1894 | // wnen.i = -vn.j*f_RMh, wnen.j = vn.i*f_RNh, wnen.k = wnen.j*tl; 1895 | wnin = wnie = wnen = O31; 1896 | sl2 = sl*sl; 1897 | gn.k = -( glv.g0*(1+5.27094e-3*sl2)-3.086e-6*pos.k ); 1898 | gcc = pgn ? *pgn : gn; 1899 | #else 1900 | this->pos = pos; this->vn = vn; 1901 | sl = sin(pos.i), cl = cos(pos.i), tl = sl/cl; 1902 | double sq = 1-e2*sl*sl, sq2 = sqrt(sq); 1903 | RMh = a*(1-e2)/sq/sq2+pos.k; f_RMh = 1.0/RMh; 1904 | RNh = a/sq2+pos.k; clRNh = cl*RNh; f_RNh = 1.0/RNh; f_clRNh = 1.0/clRNh; 1905 | wnie.i = 0.0, wnie.j = wie*cl, wnie.k = wie*sl; 1906 | wnen.i = -vn.j*f_RMh, wnen.j = vn.i*f_RNh, wnen.k = wnen.j*tl; 1907 | wnin = wnie + wnen; 1908 | sl2 = sl*sl, sl4 = sl2*sl2; 1909 | gn.k = -( glv.g0*(1+5.27094e-3*sl2+2.32718e-5*sl4)-3.086e-6*pos.k ); 1910 | gcc = pgn ? *pgn : gn; 1911 | gcc -= (wnie+wnin)*vn; 1912 | #endif 1913 | } 1914 | 1915 | CVect3 CEarth::vn2dpos(const CVect3 &vn, double ts) const 1916 | { 1917 | return CVect3(vn.j*f_RMh, vn.i*f_clRNh, vn.k)*ts; 1918 | } 1919 | 1920 | /*************************** class CIMU *********************************/ 1921 | CIMU::CIMU(void) 1922 | { 1923 | nSamples = prefirst = 1; 1924 | phim = dvbm = wm_1 = vm_1 = O31; 1925 | } 1926 | 1927 | void CIMU::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples) 1928 | { 1929 | static double conefactors[5][4] = { // coning coefficients 1930 | {2./3}, // 2 1931 | {9./20, 27./20}, // 3 1932 | {54./105, 92./105, 214./105}, // 4 1933 | {250./504, 525./504, 650./504, 1375./504} // 5 1934 | }; 1935 | int i; 1936 | double *pcf = conefactors[nSamples-2]; 1937 | CVect3 cm(0.0), sm(0.0), wmm(0.0), vmm(0.0); 1938 | 1939 | this->nSamples = nSamples; 1940 | if(nSamples==1) // one-plus-previous sample 1941 | { 1942 | if(prefirst==1) {wm_1=pwm[0]; vm_1=pvm[0]; prefirst=0;} 1943 | cm = 1.0/12*wm_1; wm_1=pwm[0]; 1944 | sm = 1.0/12*vm_1; vm_1=pvm[0]; 1945 | } 1946 | if(nSamples>1) prefirst=1; 1947 | for(i=0; iINF/2 ? 0.0 : -1.0/tauG.i; // Gyro&Acc inverse correlation time for AR(1) model 2008 | _betaGyro.j = tauG.j>INF/2 ? 0.0 : -1.0/tauG.j; 2009 | _betaGyro.k = tauG.k>INF/2 ? 0.0 : -1.0/tauG.k; 2010 | _betaAcc.i = tauA.i>INF/2 ? 0.0 : -1.0/tauA.i; 2011 | _betaAcc.j = tauA.j>INF/2 ? 0.0 : -1.0/tauA.j; 2012 | _betaAcc.k = tauA.k>INF/2 ? 0.0 : -1.0/tauA.k; 2013 | } 2014 | 2015 | void CSINS::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts) 2016 | { 2017 | #ifdef PSINS_LOW_GRADE_MEMS 2018 | #pragma message(" PSINS_LOW_GRADE_MEMS") 2019 | this->ts = ts; nts = nSamples*ts; tk += nts; 2020 | double nts2 = nts/2; 2021 | imu.Update(pwm, pvm, nSamples); 2022 | imu.phim = Kg*imu.phim - eb*nts; imu.dvbm = Ka*imu.dvbm - db*nts; // IMU calibration 2023 | // CVect3 vn01 = vn+an*nts2, pos01 = pos+eth.vn2dpos(vn01,nts2); 2024 | eth.Update(pos); 2025 | wib = imu.phim/nts; fb = imu.dvbm/nts; 2026 | web = wib; 2027 | wnb = wib; 2028 | fn = qnb*fb; 2029 | an = fn+eth.gcc; 2030 | CVect3 vn1 = vn + an*nts; 2031 | pos = pos + eth.vn2dpos(vn+vn1, nts2); vn = vn1; 2032 | Cnb0 = Cnb; 2033 | qnb = qnb*rv2q(imu.phim); 2034 | Cnb = q2mat(qnb); att = m2att(Cnb); Cbn = ~Cnb; vb = Cbn*vn; 2035 | #else 2036 | this->ts = ts; nts = nSamples*ts; tk += nts; 2037 | double nts2 = nts/2; 2038 | imu.Update(pwm, pvm, nSamples); 2039 | imu.phim = Kg*imu.phim - eb*nts; imu.dvbm = Ka*imu.dvbm - db*nts; // IMU calibration 2040 | CVect3 vn01 = vn+an*nts2, pos01 = pos+eth.vn2dpos(vn01,nts2); 2041 | eth.Update(pos01, vn01); 2042 | wib = imu.phim/nts; fb = imu.dvbm/nts; 2043 | web = wib - Cbn*eth.wnie; 2044 | wnb = wib - (qnb*rv2q(imu.phim/2))*eth.wnin; 2045 | fn = qnb*fb; 2046 | an = rv2q(-eth.wnin*nts2)*fn+eth.gcc; 2047 | CVect3 vn1 = vn + an*nts; 2048 | pos = pos + eth.vn2dpos(vn+vn1, nts2); vn = vn1; 2049 | Cnb0 = Cnb; 2050 | qnb = rv2q(-eth.wnin*nts)*qnb*rv2q(imu.phim); 2051 | Cnb = q2mat(qnb); att = m2att(Cnb); Cbn = ~Cnb; vb = Cbn*vn; 2052 | #endif 2053 | psinsassert(pos.i<85.0*PI/180 && pos.i>-85.0*PI/180); 2054 | if(vn.i>velMax) vn.i=velMax; else if(vn.i<-velMax) vn.i=-velMax; 2055 | if(vn.j>velMax) vn.j=velMax; else if(vn.j<-velMax) vn.j=-velMax; 2056 | if(vn.k>velMax) vn.k=velMax; else if(vn.k<-velMax) vn.k=-velMax; 2057 | if(pos.j>PI) pos.j-=_2PI; else if(pos.j<-PI) pos.j+=_2PI; 2058 | if(pos.k>hgtMax) pos.k=hgtMax; else if(pos.k0.1 ? vm/(nm*ts) : O31; 2138 | nm = norm(mag); // mag (in Gauss) 2139 | if(nm>0.1) 2140 | { 2141 | mag0 = mag/nm; 2142 | bxyz = Cnb*mag0; 2143 | bxyz.j = normXY(bxyz); bxyz.i = 0.0; 2144 | wxyz = (~Cnb)*bxyz; 2145 | } 2146 | else 2147 | { 2148 | mag0 = wxyz = O31; 2149 | } 2150 | exyz = *((CVect3*)&Cnb.e20)*acc0 + wxyz*mag0; 2151 | exyzInt += exyz*(Ki*ts); 2152 | qnb *= rv2q(wm-(Kp*exyz+exyzInt)*ts); 2153 | Cnb = q2mat(qnb); 2154 | tk += ts; 2155 | } 2156 | 2157 | void CMahony::Update(const CVect3 &gyro, const CVect3 &acc, const CVect3 &mag, double ts) 2158 | { 2159 | double nm; 2160 | CVect3 acc0, mag0, exyz, bxyz, wxyz; 2161 | 2162 | nm = norm(acc); 2163 | acc0 = nm>0.1 ? acc/nm : O31; 2164 | nm = norm(mag); 2165 | mag0 = nm>0.1 ? mag/nm : O31; 2166 | bxyz = Cnb*mag0; 2167 | bxyz.j = normXY(bxyz); bxyz.i = 0.0; 2168 | wxyz = (~Cnb)*bxyz; 2169 | exyz = *((CVect3*)&Cnb.e20)*acc0 + wxyz*mag0; 2170 | exyzInt += exyz*(Ki*ts); 2171 | qnb *= rv2q((gyro*glv.dps-Kp*exyz-exyzInt)*ts); 2172 | Cnb = q2mat(qnb); 2173 | tk += ts; 2174 | } 2175 | 2176 | /********************* class Quat&EKF based AHRS ************************/ 2177 | CQEAHRS::CQEAHRS(double ts):CKalman(7,3) 2178 | { 2179 | double sts = sqrt(ts); 2180 | Pmax.Set2(2.0,2.0,2.0,2.0, 1000*glv.dph,1000.0*glv.dph,1000.0*glv.dph); 2181 | Pmin.Set2(0.001,0.001,0.001,0.001, 10.0*glv.dph,10.0*glv.dph,10.0*glv.dph); 2182 | Pk.SetDiag2(1.0,1.0,1.0,1.0, 1000.0*glv.dph,1000.0*glv.dph,1000.0*glv.dph); 2183 | 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); 2184 | Rt.Set2(100.0*glv.mg/sts,100.0*glv.mg/sts, 1.0*glv.deg/sts); 2185 | Xk(0) = 1.0; 2186 | Cnb = q2mat(*(CQuat*)&Xk.dd[0]); 2187 | } 2188 | 2189 | void CQEAHRS::Update(const CVect3 &gyro, const CVect3 &acc, const CVect3 &mag, double ts) 2190 | { 2191 | double q0, q1, q2, q3, wx, wy, wz, fx, fy, fz, mx, my, mz, h11, h12, h21, h22; 2192 | q0 = Xk.dd[0], q1 = Xk.dd[1], q2 = Xk.dd[2], q3 = Xk.dd[3]; 2193 | wx = gyro.i*glv.dps, wy = gyro.j*glv.dps, wz = gyro.k*glv.dps; 2194 | fx = acc.i, fy = acc.j, fz = acc.k; 2195 | mx = mag.i, my = mag.j, mz = mag.k; 2196 | // Ft 2197 | 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; 2198 | 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; 2199 | 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; 2200 | 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; 2201 | // Hk 2202 | h11 = fx*q0-fy*q3+fz*q2; h12 = fx*q1+fy*q2+fz*q3; 2203 | h21 = fx*q3+fy*q0-fz*q1; h22 = fx*q2-fy*q1-fz*q0; 2204 | Hk.dd[ 0] = h11*2, Hk.dd[ 1] = h12*2, Hk.dd[ 2] = -h22*2, Hk.dd[ 3] = -h21*2; 2205 | Hk.dd[ 7] = h21*2, Hk.dd[ 8] = h22*2, Hk.dd[ 9] = h12*2, Hk.dd[10] = h11*2; 2206 | /* CVect3 magH = Cnb*mag; 2207 | double C11=Cnb.e11, C01=Cnb.e01, CC=C11*C11+C01*C01; 2208 | if(normXY(magH)>0.01 && CC>0.25) // CC>0.25 <=> pitch<60deg 2209 | { 2210 | double f2=2.0/CC; 2211 | 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; 2212 | Zk.dd[2] = atan2(magH.i, magH.j); 2213 | } 2214 | else 2215 | { 2216 | Hk.dd[14] = Hk.dd[15] = Hk.dd[16] = Hk.dd[17] = 0.0; 2217 | Zk.dd[2] = 0.0; 2218 | }*/ 2219 | 2220 | SetMeasFlag(0x03); 2221 | TimeUpdate(ts); 2222 | MeasUpdate(); 2223 | XPConstrain(); 2224 | normlize((CQuat*)&Xk.dd[0]); 2225 | Cnb = q2mat(*(CQuat*)&Xk.dd[0]); 2226 | } 2227 | 2228 | #endif // PSINS_AHRS_MEMS 2229 | 2230 | /****************************** File Read or Write *********************************/ 2231 | #ifdef PSINS_IO_FILE 2232 | #pragma message(" PSINS_IO_FILE") 2233 | 2234 | #include "io.h" 2235 | char* time2fname(void) 2236 | { 2237 | static char PSINSfname[32]; 2238 | time_t tt; time(&tt); 2239 | tm *Time = localtime(&tt); 2240 | strftime(PSINSfname, 32, "PSINS%Y%m%d_%H%M%S.bin", Time); 2241 | return PSINSfname; 2242 | } 2243 | 2244 | char CFileRdWt::dirIn[256] = {0}, CFileRdWt::dirOut[256] = {0}; 2245 | 2246 | void CFileRdWt::Dir(const char *dirI, const char *dirO) // set dir 2247 | { 2248 | int len = strlen(dirI); 2249 | memcpy(dirIn, dirI, len); 2250 | if(dirIn[len-1]!='\\') { dirIn[len]='\\'; dirIn[len+1]='\0'; } 2251 | if(dirO) 2252 | { 2253 | len = strlen(dirO); 2254 | memcpy(dirOut, dirO, len); 2255 | if(dirOut[len-1]!='\\') { dirOut[len]='\\'; dirOut[len+1]='\0'; } 2256 | } 2257 | else 2258 | memcpy(dirOut, dirIn, strlen(dirIn)); 2259 | if(_access(dirIn,0)==-1) dirIn[0]='\0'; 2260 | if(_access(dirOut,0)==-1) dirOut[0]='\0'; 2261 | } 2262 | 2263 | CFileRdWt::CFileRdWt() 2264 | { 2265 | f = 0; 2266 | } 2267 | 2268 | CFileRdWt::CFileRdWt(const char *fname0, int columns0) 2269 | { 2270 | Init(fname0, columns0); 2271 | memset(buff, 0, sizeof(buff)); 2272 | } 2273 | 2274 | void CFileRdWt::Init(const char *fname0, int columns0) 2275 | { 2276 | fname[0]='\0'; 2277 | int findc=0, len0=strlen(fname0); 2278 | for(int i=0; i0) // txt file read 2294 | { 2295 | f = fopen(fname, "rt"); 2296 | if(!f) return; 2297 | fpos_t pos; 2298 | while(1) // skip txt-file comments 2299 | { 2300 | pos = ftell(f); 2301 | fgets(line, sizeof(line), f); 2302 | if(feof(f)) break; 2303 | int allSpace=1, allDigital=1; 2304 | for(int i=0; i=c&&c>='0')) ) 2311 | allDigital = 0; 2312 | } 2313 | if(!allSpace && allDigital) break; 2314 | } 2315 | fsetpos(f, &pos); 2316 | this->columns = columns; 2317 | for(int i=0; i1) 2331 | fseek(f, (lines-1)*(-columns)*sizeof(double), SEEK_CUR); 2332 | fread(buff, -columns, sizeof(double), f); 2333 | } 2334 | else // txt file read 2335 | { 2336 | for(int i=0; i1) 2370 | fseek(f, (lines-1)*(-columns)*sizeof(float), SEEK_CUR); 2371 | fread(buff32, -columns, sizeof(float), f); 2372 | for(int i=0; i<-columns; i++) buff[i]=buff32[i]; 2373 | if(feof(f)) return 0; 2374 | else return 1; 2375 | } 2376 | 2377 | int CFileRdWt::getl(void) // txt file get a line 2378 | { 2379 | fgets(line, sizeof(line), f); 2380 | return strlen(line); 2381 | } 2382 | 2383 | CFileRdWt& CFileRdWt::operator<<(double d) 2384 | { 2385 | fwrite(&d, 1, sizeof(double), f); 2386 | return *this; 2387 | } 2388 | 2389 | CFileRdWt& CFileRdWt::operator<<(const CVect3 &v) 2390 | { 2391 | fwrite(&v, 1, sizeof(v), f); 2392 | return *this; 2393 | } 2394 | 2395 | CFileRdWt& CFileRdWt::operator<<(const CVect &v) 2396 | { 2397 | fwrite(v.dd, v.clm*v.row, sizeof(double), f); 2398 | return *this; 2399 | } 2400 | 2401 | CFileRdWt& CFileRdWt::operator<<(const CMat &m) 2402 | { 2403 | fwrite(m.dd, m.clm*m.row, sizeof(double), f); 2404 | return *this; 2405 | } 2406 | 2407 | CFileRdWt& CFileRdWt::operator<<(const CRAvar &R) 2408 | { 2409 | fwrite(R.R0, R.nR0, sizeof(double), f); 2410 | return *this; 2411 | } 2412 | 2413 | CFileRdWt& CFileRdWt::operator<<(const CAligni0 &aln) 2414 | { 2415 | return *this<>(double &d) 2454 | { 2455 | fread(&d, 1, sizeof(double), f); 2456 | return *this; 2457 | } 2458 | 2459 | CFileRdWt& CFileRdWt::operator>>(CVect3 &v) 2460 | { 2461 | fread(&v, 1, sizeof(v), f); 2462 | return *this; 2463 | } 2464 | 2465 | CFileRdWt& CFileRdWt::operator>>(CVect &v) 2466 | { 2467 | fread(v.dd, v.clm*v.row, sizeof(double), f); 2468 | return *this; 2469 | } 2470 | 2471 | CFileRdWt& CFileRdWt::operator>>(CMat &m) 2472 | { 2473 | fread(m.dd, m.clm*m.row, sizeof(double), f); 2474 | return *this; 2475 | } 2476 | 2477 | #endif // PSINS_IO_FILE 2478 | 2479 | #ifdef PSINS_RMEMORY 2480 | #pragma message(" PSINS_RMEMORY") 2481 | 2482 | CRMemory::CRMemory(BYTE *pMem, long memLen0, BYTE recordLen0) 2483 | { 2484 | psinsassert(recordLen0<=MAX_RECORD_BYTES); 2485 | pMemStart = pMemPush = pMemPop = pMem; 2486 | pMemEnd = pMemStart + memLen0; 2487 | pushLen = popLen = recordLen = recordLen0; 2488 | memLen = memLen0; 2489 | dataLen = 0; 2490 | } 2491 | 2492 | BYTE CRMemory::pop(BYTE *p) 2493 | { 2494 | if(dataLen==0) return 0; 2495 | popLen = recordLen==0 ? *pMemPop : recordLen; 2496 | if(p==(BYTE*)NULL) p = popBuf; 2497 | BYTE i; 2498 | for(i=0; i=pMemEnd) pMemPop = pMemStart; 2502 | } 2503 | return i; 2504 | } 2505 | 2506 | BOOL CRMemory::push(const BYTE *p) 2507 | { 2508 | BOOL res = 1; 2509 | if(p==(BYTE*)NULL) p = pushBuf; 2510 | pushLen = recordLen==0 ? *p : recordLen; 2511 | psinsassert(pushLen<=MAX_RECORD_BYTES); 2512 | for(BYTE i=0; i=pMemEnd) pMemPush = pMemStart; 2516 | if(pMemPush==pMemPop) { res=0; pop(); } 2517 | } 2518 | return res; 2519 | } 2520 | 2521 | #endif // PSINS_RMEMORY 2522 | 2523 | /*************************** class CPTimer *********************************/ 2524 | CPTimer::CPTimer(double tMax0, int isStart0, int autoReset0) 2525 | { 2526 | Start(tMax0); 2527 | isStart = isStart0, autoReset = autoReset0; 2528 | } 2529 | 2530 | void CPTimer::Start(double tMax0) 2531 | { 2532 | overflow = 0; 2533 | tCurrent = 0.0; 2534 | isStart = 1; 2535 | if(tMax0>1.0e-6) tMax=tMax0; 2536 | } 2537 | 2538 | void CPTimer::Stop(void) 2539 | { 2540 | isStart = 0; 2541 | } 2542 | 2543 | BOOL CPTimer::Update(double tStep) 2544 | { 2545 | overflow = 0; 2546 | if(isStart) 2547 | { 2548 | tCurrent += tStep; 2549 | if(tCurrent>=tMax) 2550 | { 2551 | overflow = 1; 2552 | if(autoReset) 2553 | tCurrent = 0.0; 2554 | } 2555 | } 2556 | return overflow; 2557 | } 2558 | 2559 | /*************************** function AlignCoarse *********************************/ 2560 | CVect3 Alignsb(CVect3 wmm, CVect3 vmm, double latitude) 2561 | { 2562 | double T11, T12, T13, T21, T22, T23, T31, T32, T33; 2563 | double cl = cos(latitude), tl = tan(latitude), nn; 2564 | CVect3 wbib = wmm / norm(wmm), fb = vmm / norm(vmm); 2565 | T31 = fb.i, T32 = fb.j, T33 = fb.k; 2566 | 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; 2567 | 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; 2568 | CMat3 Cnb(T11, T12, T13, T21, T22, T23, T31, T32, T33); 2569 | return m2att(Cnb); 2570 | } 2571 | 2572 | CAligni0::CAligni0(const CVect3 &pos0, const CVect3 &vel0, int velAid0) 2573 | { 2574 | eth.Update(pos0); 2575 | this->vel0 = vel0, velAid = velAid0; 2576 | tk = 0; 2577 | t0 = t1 = 10, t2 = 0; 2578 | wmm = vmm = vib0 = vi0 = Pib01 = Pib02 = Pi01 = Pi02 = O31; 2579 | qib0b = CQuat(1.0); 2580 | } 2581 | 2582 | CQuat CAligni0::Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts, const CVect3 &vel) 2583 | { 2584 | double nts = nSamples*ts; 2585 | imu.Update(pwm, pvm, nSamples); 2586 | wmm = wmm + imu.phim; vmm = vmm + imu.dvbm; 2587 | // vtmp = qib0b * (vm + 1/2 * wm X vm) 2588 | CVect3 vtmp = qib0b*imu.dvbm; 2589 | // vtmp1 = qni0' * [dvn+(wnin+wnie)Xvn-gn] * ts; 2590 | tk += nts; 2591 | CMat3 Ci0n = pos2Cen(CVect3(eth.pos.i,eth.wie*tk,0.0)); 2592 | CVect3 vtmp1 = Ci0n*(-eth.gn*nts); 2593 | if(velAid>0) 2594 | { 2595 | CVect3 dv=vel-vel0; vel0 = vel; 2596 | if(velAid==1) vtmp1 += Ci0n*dv; // for GPS vn-aided 2597 | else if(velAid==2) vtmp -= qib0b*dv+imu.phim*vel; // for OD vb-aided 2598 | } 2599 | // Pib02 = Pib02 + vib0*ts, Pi02 = Pi02 + vi0*ts 2600 | vib0 = vib0 + vtmp, vi0 = vi0 + vtmp1; 2601 | Pib02 = Pib02 + vib0*nts, Pi02 = Pi02 + vi0*nts; 2602 | // 2603 | if(++t2>3*t0) 2604 | { 2605 | t0 = t1, Pib01 = tmpPib0, Pi01 = tmpPi0; 2606 | } 2607 | else if(t2>2*t0 && t1==t0) 2608 | { 2609 | t1 = t2, tmpPib0 = Pib02, tmpPi0 = Pi02; 2610 | } 2611 | // 2612 | qib0b = qib0b*rv2q(imu.phim); 2613 | // qnb=qni0*qiib0*qib0b 2614 | if(t2<100) 2615 | { 2616 | qnb0 = qnb = CQuat(1.0); 2617 | } 2618 | else if(t2<1000) 2619 | { 2620 | qnb0 = qnb = a2qua(Alignsb(wmm, vmm, eth.pos.i)); 2621 | } 2622 | else 2623 | { 2624 | CQuat qi0ib0 = m2qua(dv2att(Pi01, Pi02, Pib01, Pib02)); 2625 | qnb0 = (~m2qua(pos2Cen(CVect3(eth.pos.i,0.0,0.0))))*qi0ib0; 2626 | qnb = (~m2qua(Ci0n))*qi0ib0*qib0b; 2627 | } 2628 | return qnb; 2629 | } 2630 | 2631 | 2632 | /*************************** class CUartPP *********************************/ 2633 | #ifdef PSINS_UART_PUSH_POP 2634 | 2635 | #pragma message(" PSINS_UART_PUSH_POP") 2636 | 2637 | CUartPP::CUartPP(int frameLen0, unsigned short head0) 2638 | { 2639 | popIdx = 0; 2640 | pushIdx = 1; 2641 | unsigned char *p = (unsigned char*)&head0; 2642 | head[0] = p[1], head[1] = p[0]; 2643 | // *(unsigned short*)head = head0; 2644 | frameLen = frameLen0; 2645 | csflag = 1, cs0 = 4, cs1 = frameLen-1, css = 2; 2646 | overflow = getframe = 0; 2647 | } 2648 | 2649 | BOOL CUartPP::checksum(const unsigned char *pc) 2650 | { 2651 | chksum = 0; 2652 | if(csflag==0) return 1; 2653 | for(int i=cs0; i<=cs1; i++) 2654 | { 2655 | if(csflag==1) chksum += pc[i]; 2656 | else if(csflag==2) chksum ^= pc[i]; 2657 | } 2658 | return chksum==pc[css]; 2659 | } 2660 | 2661 | int CUartPP::nextIdx(int idx) 2662 | { 2663 | return idx >= UARTBUFLEN - 1 ? 0 : idx + 1; 2664 | } 2665 | 2666 | int CUartPP::push(const unsigned char *buf0, int len) 2667 | { 2668 | int overflowtmp = 0; 2669 | for (int i=0; ipopIdx&&popIdx+frameLeneps) 2769 | { 2770 | s = 1; 2771 | } 2772 | else 2773 | { 2774 | s = 0; 2775 | } 2776 | return s; 2777 | } 2778 | 2779 | // set double value 'val' between range 'minVal' and 'maxVal' 2780 | double range(double val, double minVal, double maxVal) 2781 | { 2782 | double res; 2783 | 2784 | if(valmaxVal) 2789 | { 2790 | res = maxVal; 2791 | } 2792 | else 2793 | { 2794 | res = val; 2795 | } 2796 | return res; 2797 | } 2798 | 2799 | double atan2Ex(double y, double x) 2800 | { 2801 | double res; 2802 | 2803 | if((sign(y)==0) && (sign(x)==0)) 2804 | { 2805 | res = 0.0; 2806 | } 2807 | else 2808 | { 2809 | res = atan2(y, x); 2810 | } 2811 | return res; 2812 | } 2813 | 2814 | double diffYaw(double yaw, double yaw0) 2815 | { 2816 | double dyaw = yaw-yaw0; 2817 | if(dyaw>=PI) dyaw-=_2PI; 2818 | else if(dyaw<=-PI) dyaw+=_2PI; 2819 | return dyaw; 2820 | } 2821 | 2822 | double MKQt(double sR, double tau) 2823 | { 2824 | return sR*sR*2.0/tau; 2825 | } 2826 | 2827 | CVect3 MKQt(const CVect3 &sR, const CVect3 &tau) 2828 | { 2829 | 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); 2830 | } 2831 | -------------------------------------------------------------------------------- /Source/Algorithm/PSINS.h: -------------------------------------------------------------------------------- 1 | 2 | /* PSINS(Precise Strapdown Inertial Navigation System) C++ alogrithm hearder file PSINS.h */ 3 | /* 4 | Copyright(c) 2015-2018, by Gongmin Yan, All rights reserved. 5 | Northwestern Polytechnical University, Xi'an, P.R.China 6 | Date: 17/02/2015, 19/07/2017, 08/10/2018 7 | */ 8 | 9 | #ifndef _PSINS_H 10 | #define _PSINS_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | /************** compile control !!! ***************/ 21 | //#define MAT_COUNT_STATISTIC 22 | #define PSINS_IO_FILE 23 | //#define PSINS_RMEMORY 24 | #define PSINS_AHRS_MEMS 25 | //#define PSINS_LOW_GRADE_MEMS 26 | //#define CSINSGPSOD_INIT_BY_USER 27 | //#define PSINS_psinsassert 28 | //#define PSINS_UART_PUSH_POP 29 | 30 | #define disp(k, nSec, frq) if(k%(nSec*frq)==0) printf("%d\n", k/frq) 31 | 32 | // type re-define 33 | #ifndef BOOL 34 | typedef int BOOL; 35 | #endif 36 | 37 | #ifndef BYTE 38 | typedef unsigned char BYTE; 39 | #endif 40 | 41 | // constant define 42 | #ifndef TRUE 43 | #define TRUE 1 44 | #define FALSE 0 45 | #endif 46 | 47 | #ifndef NULL 48 | #define NULL ((void *)0) 49 | #endif 50 | 51 | #ifndef PI 52 | #define PI 3.14159265358979 53 | #endif 54 | #define PI_2 (PI/2.0) 55 | #define PI_4 (PI/4.0) 56 | #define _2PI (2.0*PI) 57 | 58 | #ifndef DEG 59 | #define DEG (PI/180.0) 60 | #endif 61 | #define DPS (DEG/1.0) // deg/s 62 | #define SEC (DEG/3600.0) // arcsec 63 | #define DPH (DEG/3600.0) // deg/h 64 | #define G0 9.7803267714 65 | #define UG (G0/1.0e6) // ug 66 | #define RE 6378137.0 67 | 68 | #ifndef EPS 69 | #define EPS 2.220446049e-16F 70 | #endif 71 | #ifndef INF 72 | #define INF 3.402823466e+30F 73 | #endif 74 | 75 | #ifdef PSINS_psinsassert 76 | BOOL psinspsinsassert(BOOL b); 77 | #else 78 | #define psinsassert(b) {}; 79 | #endif 80 | 81 | #ifndef max 82 | #define max(x,y) ( (x)>=(y)?(x):(y) ) 83 | #endif 84 | #ifndef min 85 | #define min(x,y) ( (x)<=(y)?(x):(y) ) 86 | #endif 87 | 88 | #define CC180toC360(yaw) ( (yaw)>0.0 ? (_2PI-(yaw)) : -(yaw) ) // counter-clockwise +-180deg -> clockwise 0~360deg for yaw 89 | #define C360toCC180(yaw) ( (yaw)>=PI ? (_2PI-(yaw)) : -(yaw) ) // clockwise 0~360deg -> counter-clockwise +-180deg for yaw 90 | 91 | // class define 92 | class CGLV; 93 | class CVect3; class CMat3; class CQuat; 94 | class CEarth; class CIMU; class CSINS; class CAligni0; 95 | class CVect; class CMat; class CKalman; class CSINSKF; class CSINSTDKF; class CSINSGPS; 96 | class CIIR; class CIIRV3; class CRMemory; class CFileRdWt; class CUartPP; 97 | 98 | // function define 99 | double r2dm(double r); 100 | double dm2r(double dm); 101 | int sign(double val, double eps=EPS); 102 | double range(double val, double minVal, double maxVal); 103 | double atan2Ex(double y, double x); 104 | double diffYaw(double yaw, double yaw0); 105 | double MKQt(double sR, double tau); 106 | #define pow2(x) ((x)*(x)) 107 | #define asinEx(x) asin(range(x, -1.0, 1.0)) 108 | #define acosEx(x) acos(range(x, -1.0, 1.0)) 109 | CVect3 q2att(const CQuat &qnb); 110 | CMat3 diag(const CVect3 &v); 111 | void IMURFU(CVect3 *pwm, int nSamples, const char *str); 112 | void IMURFU(CVect3 *pwm, CVect3 *pvm, int nSamples, const char *str); 113 | 114 | // Max Matrix Dimension define 115 | #define MMD 20 116 | #define MMD2 (MMD*MMD) 117 | 118 | // global variables and functions 119 | extern const CVect3 I31, O31, Ipos; 120 | extern const CQuat qI; 121 | extern const CMat3 I33, O33; 122 | extern const CVect On1, O1n; 123 | extern const CGLV glv; 124 | 125 | #define LLH(latitude,longitude,height) CVect3((latitude)*PI/180,(longitude)*PI/180,height) 126 | #define PRY(pitch,roll,yaw) CVect3((pitch)*PI/180,(roll)*PI/180,(yaw)*PI/180) 127 | 128 | class CGLV 129 | { 130 | public: 131 | double Re, f, g0, wie; // the Earth's parameters 132 | double e, e2, ep, ep2, Rp; 133 | double mg, ug, deg, min, sec, hur, ppm, ppmpsh; // commonly used units 134 | double dps, dph, dpsh, dphpsh, ugpsh, ugpsHz, mpsh, mpspsh, secpsh; 135 | 136 | CGLV(double Re=6378137.0, double f=(1.0/298.257), double wie0=7.2921151467e-5, double g0=9.7803267714); 137 | }; 138 | 139 | class CVect3 140 | { 141 | public: 142 | double i, j, k; 143 | 144 | CVect3(void); 145 | CVect3(double xyz); 146 | CVect3(double xx, double yy, double zz); 147 | CVect3(const double *pdata); 148 | 149 | CVect3& operator=(double f); // every element equal to a same double 150 | friend BOOL IsZero(const CVect3 &v, double eps=EPS); // psinsassert if all elements are zeros 151 | friend BOOL IsZeroXY(const CVect3 &v, double eps=EPS); // psinsassert if x&&y-elements are zeros 152 | friend BOOL IsNaN(const CVect3 &v); // psinsassert if any element is NaN 153 | CVect3 operator+(const CVect3 &v) const; // vector addition 154 | CVect3 operator-(const CVect3 &v) const; // vector subtraction 155 | CVect3 operator*(const CVect3 &v) const; // vector cross multiplication 156 | CVect3 operator*(const CMat3 &m) const; // row-vector multiply matrix 157 | CVect3 operator*(double f) const; // vector multiply scale 158 | CVect3 operator/(double f) const; // vector divide scale 159 | CVect3 operator/(const CVect3 &v) const; // vector divide vect3 element by element 160 | CVect3& operator=(const double *pf); // vector equal to a array 161 | CVect3& operator+=(const CVect3 &v); // vector addition 162 | CVect3& operator-=(const CVect3 &v); // vector subtraction 163 | CVect3& operator*=(double f); // vector multiply scale 164 | CVect3& operator/=(double f); // vector divide scale 165 | CVect3& operator/=(const CVect3 &v); // vector divide vect3 element by element 166 | friend CVect3 operator*(double f, const CVect3 &v); // scale multiply vector 167 | friend CVect3 operator-(const CVect3 &v); // minus 168 | friend CVect3 abs(const CVect3 &v); // abs 169 | friend double norm(const CVect3 &v); // vector norm 170 | friend double normXY(const CVect3 &v); // vector norm of X & Y components 171 | friend CVect3 sqrt(const CVect3 &v); // sqrt 172 | friend CVect3 pow(const CVect3 &v, int k=2); // power 173 | friend double dot(const CVect3 &v1, const CVect3 &v2); // vector dot multiplication 174 | friend CMat3 a2mat(const CVect3 &att); // Euler angles to DCM 175 | friend CVect3 m2att(const CMat3 &Cnb); // DCM to Euler angles 176 | friend CQuat a2qua(double pitch, double roll, double yaw); // Euler angles to quaternion 177 | friend CQuat a2qua(const CVect3 &att); // Euler angles to quaternion 178 | friend CVect3 q2att(const CQuat &qnb); // quaternion to Euler angles 179 | friend CQuat rv2q(const CVect3 &rv); // rotation vector to quaternion 180 | friend CVect3 q2rv(const CQuat &q); // quaternion to rotation vector 181 | friend CMat3 askew(const CVect3 &v); // askew matrix; 182 | friend CMat3 pos2Cen(const CVect3 &pos); // to geographical position matrix 183 | friend CVect3 pp2vn(const CVect3 &pos1, const CVect3 &pos0, double ts=1.0, CEarth *pEth=(CEarth*)NULL); // position difference to velocity 184 | friend CVect3 MKQt(const CVect3 &sR, const CVect3 &tau);// first order Markov white-noise variance calculation 185 | friend CMat3 dv2att(CVect3 &va1, const CVect3 &va2, CVect3 &vb1, const CVect3 &vb2); // attitude determination using double-vector 186 | friend CVect3 Alignsb(CVect3 wmm, CVect3 vmm, double latitude); // align in static-base 187 | friend double MagYaw(const CVect3 &mag, const CVect3 &att, double declination=0); 188 | friend CVect3 xyz2blh(const CVect3 &xyz); // ECEF X/Y/Z to latitude/longitude/height 189 | friend CVect3 blh2xyz(const CVect3 &blh); // latitude/longitude/height to ECEF X/Y/Z 190 | friend CVect3 Vxyz2enu(const CVect3 &Vxyz, const CVect3 &pos); // ECEF Vx/Vy/Vz to Ve/Vn/Vu 191 | }; 192 | 193 | class CQuat 194 | { 195 | public: 196 | double q0, q1, q2, q3; 197 | 198 | CQuat(void); 199 | CQuat(double qq0, double qq1=0.0, double qq2=0.0, double qq3=0.0); 200 | CQuat(const double *pdata); 201 | 202 | CQuat operator+(const CVect3 &phi) const; // true quaternion add misalign angles 203 | CQuat operator-(const CVect3 &phi) const; // calculated quaternion delete misalign angles 204 | CVect3 operator-(CQuat &quat) const; // get misalign angles from calculated quaternion & true quaternion 205 | CQuat operator*(const CQuat &q) const; // quaternion multiplication 206 | CVect3 operator*(const CVect3 &v) const; // quaternion multiply vector 207 | CQuat& operator*=(const CQuat &q); // quaternion multiplication 208 | CQuat& operator-=(const CVect3 &phi); // calculated quaternion delete misalign angles 209 | void SetYaw(double yaw=0.0); // set Euler angles to designated yaw 210 | friend void normlize(CQuat *q); // quaternion norm 211 | friend CQuat operator~(const CQuat &q); // quaternion conjugate 212 | friend CQuat UpDown(const CQuat &q); // Up-Down the quaternion 213 | }; 214 | 215 | class CMat3 216 | { 217 | public: 218 | double e00, e01, e02, e10, e11, e12, e20, e21, e22; 219 | 220 | CMat3(void); 221 | CMat3(double xx, double xy, double xz, 222 | double yx, double yy, double yz, 223 | double zx, double zy, double zz ); 224 | CMat3(const CVect3 &v0, const CVect3 &v1, const CVect3 &v2); // M = [v0; v1; v2] 225 | 226 | CMat3 operator+(const CMat3 &m) const; // matirx addition 227 | CMat3 operator-(const CMat3 &m) const; // matirx subtraction 228 | CMat3 operator*(const CMat3 &m) const; // matirx multiplication 229 | CMat3 operator*(double f) const; // matirx multiply scale 230 | CVect3 operator*(const CVect3 &v) const; // matirx multiply vector 231 | friend CMat3 operator-(const CMat3 &m); // minus 232 | friend CMat3 operator~(const CMat3 &m); // matirx transposition 233 | friend CMat3 operator*(double f, const CMat3 &m); // scale multiply matirx 234 | friend double det(const CMat3 &m); // matirx determination 235 | friend CMat3 inv(const CMat3 &m); // matirx inverse 236 | friend CVect3 diag(const CMat3 &m); // diagonal of a matrix 237 | friend CMat3 diag(const CVect3 &v); // diagonal matrix 238 | friend CQuat m2qua(const CMat3 &Cnb); // DCM to quaternion 239 | friend CMat3 q2mat(const CQuat &qnb); // quaternion to DCM 240 | }; 241 | 242 | class CVect 243 | { 244 | public: 245 | int row, clm, rc; 246 | double dd[MMD]; 247 | 248 | CVect(void); 249 | CVect(int row0, int clm0=1); 250 | CVect(int row0, double f); 251 | CVect(int row0, double f, double f1, ...); 252 | CVect(int row0, const double *pf); 253 | CVect(const CVect3 &v); 254 | CVect(const CVect3 &v1, const CVect3 v2); 255 | 256 | void Set(double f, ...); 257 | void Set2(double f, ...); 258 | CVect operator+(const CVect &v) const; // vector addition 259 | CVect operator-(const CVect &v) const; // vector subtraction 260 | CVect operator*(double f) const; // vector multiply scale 261 | CVect& operator=(double f); // every element equal to a same double 262 | CVect& operator=(const double *pf); // vector equal to a array 263 | CVect& operator+=(const CVect &v); // vector addition 264 | CVect& operator-=(const CVect &v); // vector subtraction 265 | CVect& operator*=(double f); // vector multiply scale 266 | CVect operator*(const CMat &m) const; // row-vector multiply matrix 267 | CMat operator*(const CVect &v) const; // 1xn vector multiply nx1 vector, or nx1 vector multiply 1xn vector 268 | double& operator()(int r); // vector element 269 | friend CVect operator~(const CVect &v); // vector transposition 270 | friend CVect abs(const CVect &v); // vector abs 271 | friend double norm(const CVect &v); // vector norm 272 | friend CVect pow(const CVect &v, int k=2); // power 273 | }; 274 | 275 | class CMat 276 | { 277 | public: 278 | int row, clm, rc; 279 | double dd[MMD2]; 280 | 281 | CMat(void); 282 | CMat(int row0, int clm0); 283 | CMat(int row0, int clm0, double f); 284 | CMat(int row0, int clm0, const double *pf); 285 | 286 | void SetDiag(double f, ...); 287 | void SetDiag2(double f, ...); 288 | CMat operator+(const CMat &m) const; // matirx addition 289 | CMat operator-(const CMat &m) const; // matirx subtraction 290 | CMat operator*(double f) const; // matirx multiply scale 291 | CVect operator*(const CVect &v) const; // matirx multiply vector 292 | CMat operator*(const CMat &m) const; // matirx multiplication 293 | CMat& operator+=(const CMat &m0); // matirx addition 294 | CMat& operator+=(const CVect &v); // matirx + diag(vector) 295 | CMat& operator-=(const CMat &m0); // matirx subtraction 296 | CMat& operator*=(double f); // matirx multiply scale 297 | CMat& operator++(); // 1.0 + diagonal 298 | double& operator()(int r, int c); // get element m(r,c) 299 | void ZeroRow(int i); // set i-row to 0 300 | void ZeroClm(int j); // set j-column to 0 301 | void SetRow(int i, double f, ...); 302 | void SetRow(int i, const CVect &v); // set i-row from vector 303 | void SetClm(int j, double f, ...); 304 | void SetClm(int j, const CVect &v); // set j-column from vector 305 | CVect GetRow(int i) const; // get i-row from matrix 306 | CVect GetClm(int j) const; // get j-column from matrix 307 | void SetRowVect3(int i, int j, const CVect3 &v); // set i-row&j...(j+2)-column from CVect3 308 | void SetClmVect3(int i, int j, const CVect3 &v); // set i...(i+2)-row&j-column from CVect3 309 | 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; 310 | void SetMat3(int i, int j, const CMat3 &m); // set i...(i+2)-row&j...(j+2)-comumn from CMat3 311 | CMat3 GetMat3(int i, int j) const; // get CMat3 from i...(i+2)-row&j...(j+2)-comumn 312 | void SubAddMat3(int i, int j, const CMat3 &m); // add i...(i+2)-row&j...(j+2)-comumn with CMat3 m 313 | friend CMat operator~(const CMat &m); // matirx transposition 314 | friend void symmetry(CMat &m); // matirx symmetrization 315 | friend double norm1(CMat &m); // 1-norm 316 | friend CVect diag(const CMat &m); // diagonal of a matrix 317 | friend CMat diag(const CVect &v); // diagonal matrix 318 | friend void RowMul(CMat &m, const CMat &m0, const CMat &m1, int r); // m(r,:)=m0(r,:)*m1 319 | friend void RowMulT(CMat &m, const CMat &m0, const CMat &m1, int r); // m(r,:)=m0(r,:)*m1' 320 | friend void DVMDVafa(const CVect &V, CMat &M, double afa=1.0); // M = diag(V)*M*diag(V)*afa 321 | #ifdef MAT_COUNT_STATISTIC 322 | static int iCount, iMax; 323 | ~CMat(void); 324 | #endif 325 | }; 326 | 327 | class CRAvar 328 | { 329 | public: 330 | #define RAMAX MMD 331 | int nR0, Rmaxcount[RAMAX], Rmaxflag[RAMAX]; 332 | double ts, R0[RAMAX], Rmax[RAMAX], Rmin[RAMAX], tau[RAMAX], r0[RAMAX]; 333 | 334 | CRAvar(void); 335 | CRAvar(int nR0, int maxCount0=2); 336 | void set(double r0, double tau, double rmax=0.0, double rmin=0.0, int i=0); 337 | void set(const CVect3 &r0, const CVect3 &tau, const CVect3 &rmax=O31, const CVect3 &rmin=O31); 338 | void set(const CVect &r0, const CVect &tau, const CVect &rmax=On1, const CVect &rmin=On1); 339 | void Update(double r, double ts, int i=0); 340 | void Update(const CVect3 &r, double ts); 341 | void Update(const CVect &r, double ts); 342 | double operator()(int k); // get element sqrt(R0(k)) 343 | }; 344 | 345 | class CVAR 346 | { 347 | public: 348 | #define VARMAX 50 349 | int ipush, imax; 350 | float array[VARMAX], mean, var; 351 | 352 | CVAR(int imax0=10, float data0=0.0); 353 | float Update(float data); 354 | }; 355 | 356 | class CVARn { 357 | public: 358 | int row, clm, idxpush, rowcnt; 359 | double **pData, *pd, *Sx, *Sx2, *mx, *stdx; // sum(x), sum(x^2), mean(x), std(x) 360 | double stdsf; // std scalefactor 361 | CVARn(void); 362 | CVARn(int row0, int clm0); 363 | ~CVARn(void); 364 | void Reset(void); 365 | BOOL Update(const double *pd); 366 | BOOL Update(double f, ...); 367 | }; 368 | 369 | class CEarth 370 | { 371 | public: 372 | double a, b; 373 | double f, e, e2; 374 | double wie; 375 | 376 | double sl, sl2, sl4, cl, tl, RMh, RNh, clRNh, f_RMh, f_RNh, f_clRNh; 377 | CVect3 pos, vn, wnie, wnen, wnin, gn, gcc, *pgn; 378 | 379 | CEarth(double a0=glv.Re, double f0=glv.f, double g0=glv.g0); 380 | void Update(const CVect3 &pos, const CVect3 &vn=O31); 381 | CVect3 vn2dpos(const CVect3 &vn, double ts=1.0) const; 382 | }; 383 | 384 | class CEGM // Earth Gravitational Model 385 | { 386 | public: 387 | CEGM(void); 388 | int Init(const char *fileModel); 389 | int Init(double GM0, double Re0, double ff0, double wie0, double *pC0, double *pS0, int N0); 390 | ~CEGM(void); 391 | void Update(double *blh, double *gn, int degree=-1); 392 | }; 393 | 394 | class CIMU 395 | { 396 | public: 397 | int nSamples, prefirst; 398 | CVect3 phim, dvbm, wm_1, vm_1; 399 | 400 | CIMU(void); 401 | void Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples); 402 | friend void IMURFU(CVect3 *pwm, int nSamples, const char *str); 403 | friend void IMURFU(CVect3 *pwm, CVect3 *pvm, int nSamples, const char *str); 404 | }; 405 | 406 | class CSINS 407 | { 408 | public: 409 | double ts, nts, tk, velMax, hgtMin, hgtMax; 410 | CEarth eth; 411 | CIMU imu; 412 | CQuat qnb; 413 | CMat3 Cnb, Cnb0, Cbn, Kg, Ka; 414 | CVect3 wib, fb, fn, an, web, wnb, att, vn, vb, pos, eb, db, tauGyro, tauAcc, _betaGyro, _betaAcc; 415 | CMat3 Maa, Mav, Map, Mva, Mvv, Mvp, Mpv, Mpp; // for etm 416 | CVect3 lvr, vnL, posL; CMat3 CW, MpvCnb; // for lever arm 417 | CQuat qnbE; CVect3 attE, vnE, posE; // for extrapolation 418 | CRAvar Rwfa; 419 | 420 | CSINS(const CQuat &qnb0=qI, const CVect3 &vn0=O31, const CVect3 &pos0=O31, double tk0=0.0); // initialization using quat attitude, velocity & position 421 | void SetTauGA(const CVect3 &tauG, const CVect3 &tauA); 422 | void Update(const CVect3 *pwm, const CVect3 *pvm, int nSamples, double ts); // SINS update using Gyro&Acc samples 423 | void Extrap(const CVect3 &wm=O31, const CVect3 &vm=O31, double ts=0.0); // SINS fast extrapolation using 1 Gyro&Acc sample 424 | void lever(const CVect3 &dL=O31); // lever arm 425 | void etm(void); // SINS error transform matrix coefficients 426 | }; 427 | 428 | class CIIR 429 | { 430 | public: 431 | #define IIRnMax 6 432 | int n; 433 | double b[IIRnMax], a[IIRnMax], x[IIRnMax], y[IIRnMax]; 434 | 435 | CIIR(void); 436 | CIIR(double *b0, double *a0, int n0); 437 | double Update(double x0); 438 | }; 439 | 440 | class CIIRV3 441 | { 442 | public: 443 | CIIR iir0, iir1, iir2; 444 | CVect3 y; 445 | 446 | CIIRV3(void); 447 | CIIRV3(double *b0, double *a0, int n0, 448 | double *b1=(double*)NULL, double *a1=(double*)NULL, int n1=0, 449 | double *b2=(double*)NULL, double *a2=(double*)NULL, int n2=0); 450 | CVect3 Update(CVect3 &x); 451 | }; 452 | 453 | class CAligni0 454 | { 455 | public: 456 | int velAid, t0, t1, t2; 457 | CVect3 vel0, wmm, vmm, vib0, vi0, Pib01, Pib02, Pi01, Pi02, tmpPib0, tmpPi0; 458 | CQuat qib0b; 459 | CEarth eth; 460 | CIMU imu; 461 | double tk; 462 | CQuat qnb0, qnb; 463 | 464 | CAligni0(const CVect3 &pos0=O31, const CVect3 &vel0=O31, int velAid=0); 465 | CQuat Update(const CVect3 *wm, const CVect3 *vm, int nSamples, double ts, const CVect3 &vel=O31); 466 | }; 467 | 468 | class CKalman 469 | { 470 | public: 471 | double kftk, zfdafa; 472 | int nq, nr, measflag; 473 | CMat Ft, Pk, Hk, Fading; 474 | CVect Xk, Zk, Qt, Rt, rts, Xmax, Pmax, Pmin, Zfd, Zfd0, 475 | Rmax, Rmin, Rbeta, Rb, // measurement noise R adaptive 476 | FBTau, FBMax, FBXk, FBTotal; // feedback control 477 | int Rmaxcount[MMD], Rmaxcount0[MMD]; 478 | // CRAvar Ravar; 479 | 480 | CKalman(int nq0, int nr0); 481 | virtual void Init(void) = 0; // initialize Qk,Rk,P0... 482 | virtual void SetFt(void) = 0; // process matrix setting 483 | virtual void SetHk(void) = 0; // measurement matrix setting 484 | virtual void SetMeas(void) = 0; // set measurement 485 | virtual void Feedback(double fbts); // feed back 486 | void TimeUpdate(double kfts, int fback=1); // time update 487 | int MeasUpdate(double fading=1.0); // measurement update 488 | int RAdaptive(int i, double r, double Pr); // Rt adaptive 489 | void RPkFading(int i); // multiple fading 490 | void SetMeasFlag(int flag); // measurement flag setting 491 | void XPConstrain(void); // Xk & Pk constrain: -Xmax>(double &d); 601 | CFileRdWt& operator>>(CVect3 &v); 602 | CFileRdWt& operator>>(CVect &v); 603 | CFileRdWt& operator>>(CMat &m); 604 | ~CFileRdWt(); 605 | }; 606 | 607 | char* time2fname(void); 608 | 609 | #endif // PSINS_IO_FILE 610 | 611 | #ifdef PSINS_RMEMORY 612 | 613 | #define MAX_RECORD_BYTES 128 614 | 615 | class CRMemory 616 | { 617 | BYTE *pMemStart, *pMemEnd; 618 | BYTE pushLen, popLen, recordLen; 619 | long memLen, dataLen; 620 | public: 621 | BYTE *pMemPush, *pMemPop, pushBuf[MAX_RECORD_BYTES], popBuf[MAX_RECORD_BYTES]; 622 | 623 | CRMemory(BYTE *pMem, long memLen0, BYTE recordLen0=0); 624 | BOOL push(const BYTE *p=(const BYTE*)NULL); 625 | BYTE pop(BYTE *p=(BYTE*)NULL); 626 | }; 627 | 628 | #endif // PSINS_RMEMORY 629 | 630 | #ifdef PSINS_UART_PUSH_POP 631 | 632 | class CUartPP 633 | { 634 | public: 635 | #define UARTFRMLEN (50*4) 636 | #define UARTBUFLEN (UARTFRMLEN*20) 637 | unsigned char head[2], popbuf[UARTFRMLEN], buf[UARTBUFLEN], chksum; 638 | int pushIdx, popIdx, frameLen, overflow, getframe; 639 | int csflag, cs0, cs1, css; // 0: no checksum, 1: unsigned char sum, 2: crc8; popbuf[css]==popbuf[cs0]+...+popbuf[cs1] 640 | //unsigned short chksum; 641 | 642 | BOOL checksum(const unsigned char *pc); 643 | CUartPP(int frameLen0, unsigned short head0=0x55aa); // NOTE: char {0xaa 0x55} 644 | int nextIdx(int idx); 645 | int push(const unsigned char *buf0, int len); 646 | int pop(unsigned char *buf0=(unsigned char*)NULL); 647 | }; 648 | 649 | #endif // PSINS_UART_PUSH_POP 650 | 651 | class CPTimer // PSINS Timer 652 | { 653 | double tCurrent, tMax; 654 | int isStart, autoReset; 655 | 656 | public: 657 | BOOL overflow; 658 | CPTimer(double tMax0=1.0, BOOL isStart0=0, BOOL autoReset0=0); 659 | void Start(double tMax0=0.0); 660 | void Stop(void); 661 | BOOL Update(double tStep); 662 | }; 663 | 664 | typedef struct { 665 | unsigned short head, chksum; 666 | float t, gx, gy, gz, ax, ay, az, magx, magy, magz, bar, pch, rll, yaw, ve, vn, vu, 667 | lon0, lon1, lat0, lat1, hgt, gpsve, gpsvn, gpsvu, gpslon0, gpslon1, gpslat0, gpslat1, gpshgt, 668 | gpsstat, gpsdly, tmp, rsv; 669 | } PSINSBoard; 670 | 671 | #endif 672 | -------------------------------------------------------------------------------- /Source/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #Templates are created by Cattle 2018.12.2 2 | 3 | cmake_minimum_required (VERSION 2.8) 4 | PROJECT(PSINS) 5 | 6 | SET(CMAKE_C_COMPILER "/.....-gcc ") 7 | SET(CMAKE_CXX_COMPILER "/.....-g++ ") 8 | 9 | 10 | #find_package(Threads REQUIRED) 11 | 12 | 13 | 14 | 15 | include_directories(...) 16 | 17 | 18 | ADD_SUBDIRECTORY(lib) 19 | 20 | -------------------------------------------------------------------------------- /readme.txt: -------------------------------------------------------------------------------- 1 | 2018.12.1 Create repository! --------------------------------------------------------------------------------