diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 89 |
1 files changed, 18 insertions, 71 deletions
diff --git a/src/body.cpp b/src/body.cpp index 5f5076b..fdd41a9 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -115,101 +115,48 @@ Body::R() Matrix<double,STATESIZE,STATESIZE> Body::Q (double dt) { - const bool doang=true; - const bool doquat=true; - Matrix<double,STATESIZE,3> Ga; Ga.block<3,3>(0,0) = 0.5*dt*dt*Matrix3d::Identity(); Ga.block<3,3>(3,0) = dt*Matrix3d::Identity(); Ga.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero(); Matrix<double,STATESIZE,3> Gb; - if (doang) { + if (donew) { Gb.block<3,3>(0,0) = -0.5*dt*dt*Matrix3d::Identity(); Gb.block<3,3>(3,0) = -dt*Matrix3d::Identity(); Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero(); + Gb.block<3,3>(STATESIZE-3,0) = dt*Matrix3d::Identity(); } else { Gb.block<STATESIZE-3,3>(0,0) = Matrix<double,STATESIZE-3,3>::Zero(); + Gb.block<3,3>(STATESIZE-3,0) = 0.5*dt*dt*Matrix3d::Identity(); } - Gb.block<3,3>(STATESIZE-3,0) = dt*Matrix3d::Identity(); Matrix<double,STATESIZE,3> Gw; - Gw.block<3,3>(0,0) = 0.5*dt*dt*skewSymmetric(vel()); - Gw.block<3,3>(3,0) = dt*skewSymmetric(vel()); - Gw.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero(); - -#if STATESIZE==13 - Matrix<double,STATESIZE,Eigen::Dynamic> Gq; - if (doquat) { - Gq = Matrix<double,STATESIZE,3>::Zero(); - Quaterniond q = qhat(); - - Gq.block<6,3>(0,0) = Matrix<double,6,3>::Zero(); - Gq.block<4,3>(6,0) = 0.5*dt*qomega(q); - Gq.block<3,3>(10,0) = Matrix<double,3,3>::Zero(); + if (donew) { + Gw.block<3,3>(0,0) = 0.5*dt*dt*skewSymmetric(vel()); + Gw.block<3,3>(3,0) = dt*skewSymmetric(vel()); + Gw.block<3,3>(STATESIZE-3,0) = Matrix3d::Zero(); } else { - Gq = Matrix<double,STATESIZE,4>::Zero(); - Gq.block<6,4>(0,0) = Matrix<double,6,4>::Zero(); - Gq.block<4,4>(6,0) = dt*Matrix<double,4,4>::Identity(); - Gq.block<3,4>(10,0) = Matrix<double,3,4>::Zero(); + Gw.block<6,0>(0,0) = Matrix<double,6,0>::Zero(); + Gw.block<3,3>(STATESIZE-3,0) = Matrix3d::Zero(); } + +#if STATESIZE==13 + Quaterniond q = qhat(); + Gw.block<4,3>(6,0) = 0.5*dt*qomega(q); #endif Matrix<double,STATESIZE,STATESIZE> Q; Q = Matrix<double,STATESIZE,STATESIZE>::Zero(); - if (doang) { + if (donew) { Q += acc_std*acc_std*Ga*Ga.transpose(); - //Q += ang_std*ang_std*Gw*Gw.transpose(); + Q += ang_std*ang_std*Gw*Gw.transpose(); + Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose(); } else { Q += (acc_bias_std+ang_std+acc_std)*(acc_std+acc_bias_std+ang_std)*Ga*Ga.transpose(); + Q += ang_std*ang_std*Gw*Gw.transpose(); + Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose(); } - Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose(); - -#if STATESIZE==13 - Q += ang_std*ang_std *(Gq+Gw) * (Gq+Gw).transpose(); -#endif - - - /* - Matrix<double,STATESIZE,STATESIZE> Q; - Q = Matrix<double,STATESIZE,STATESIZE>::Zero(); - // Qxx - Q.block<3,3>(0,0) = 0.25*dt*dt*dt*dt*ACC_STD*ACC_STD*Matrix<double,3,3>::Identity(); - - // Qxv, Qvx - Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix3d::Identity()-0.25*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity(); - Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix3d::Identity()-0.25*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity(); - - //Qvv - Q.block<3,3>(3,3) = dt*dt*(ACC_STD*ACC_STD+0.25*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix<double,3,3>::Identity(); - -#if STATESIZE==13 - // Qbb - Q.block<3,3>(10,10) = dt*dt*ACC_BIAS_STD*ACC_BIAS_STD*Matrix<double,3,3>::Identity(); - - // Qxb, Qbx - Q.block<3,3>(10,0) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity(); - Q.block<3,3>(0,10) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity(); - - // Qvb, Qbv - Q.block<3,3>(3,10) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity(); - Q.block<3,3>(10,3) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity(); - - // Qqq - Q.block<4,4>(6,6) = 0.25*dt*dt*ANG_STD*ANG_STD*Matrix<double,4,4>::Identity(); -#else - // Qbb - Q.block<3,3>(6,6) = dt*dt*ACC_BIAS_STD*ACC_BIAS_STD*Matrix<double,3,3>::Identity(); - - // Qxb, Qbx - Q.block<3,3>(6,0) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity(); - Q.block<3,3>(0,6) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity(); - - // Qvb, Qbv - Q.block<3,3>(3,6) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity(); - Q.block<3,3>(6,3) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity(); -#endif -*/ return Q; } /* ----- end of method Body::q ----- */ |