void KalmanFilter<DataTypes, DepthTypes>::predict(vpMatrix& stiffnessMatrix) { predictedForces = estimatedForces; predictedPositions = estimatedPositions + stiffnessMatrix.pseudoInverse()*estimatedForces; for (int k = 0; k < 2*predictedForces.getCols(); k++) for (int l = 0; l < 2*predictedForces.getCols(); l++) std::cout <<k<<" "<<l<<" "<< stiffnessMatrix[k][l] << std::endl; estimatedState = estimatedPositions; estimatedState.stack(estimatedForces); predictedState = predictedPositions; predictedState.stack(predictedForces); for (int k = predictedForces.getCols(); k < 2*predictedForces.getCols(); k++) for (int l = predictedForces.getCols(); l < 2*predictedForces.getCols(); l++) J[k][l] = stiffnessMatrix[k-predictedForces.getCols()][l-predictedForces.getCols()]; PPred = J*PEst*J.transpose() + Q; //PfPred = PsEst + Qf; }
vpColVector computeSecondaryTaskManipulability(const vpMatrix &P, vpMatrix &J, std::vector <vpMatrix> &dJ,double & cond) { const unsigned int n = dJ.size(); vpColVector z(n); double alpha = 10; // vpMatrix v; // vpColVector w; // J.svd(w, v); // //std::cout << "singular values:/n" << w << std::endl; // cond = w[0]/w[5]; // double sqtr_detJJt = 1.0; // for (unsigned int i = 0; i < n-1; i++) // sqtr_detJJt *= w[i]; // std::cout << "sqtr_detJJt:/n" << sqtr_detJJt << std::endl; // std::cout << "detJJt" << detJJt << std::endl; double detJJt = (J * J.transpose()).det(); std::cout << "sqtr_detJJMulti" << sqrt(detJJt) << std::endl; for (unsigned int i = 0; i < n; i++) { vpMatrix dJJinv = dJ[i] * J.pseudoInverse(); std::cout << dJ[i] << std::endl << dJ[i] << std::endl; double trace = 0.0; for (unsigned int k = 0; k < dJJinv.getCols(); k++) trace += dJJinv[k][k]; std::cout << "trace" << i << " " << trace << std::endl; z[i] = alpha * sqrt(detJJt) * trace; } return z; }
void vpMbKltTracker::computeVVSPoseEstimation(const unsigned int iter, vpMatrix &L, const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true, vpColVector &R, vpMatrix <L, vpColVector <R, vpColVector &error_prev, vpColVector &v, double &mu, vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev) { m_error = R; if(computeCovariance){ L_true = L; if(!isoJoIdentity){ vpVelocityTwistMatrix cVo; cVo.buildFrom(cMo); LVJ_true = (L*cVo*oJo); } } normRes_1 = normRes; normRes = 0; for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){ w_true[i] = w[i]; R[i] = R[i] * w[i]; normRes += R[i]; } if((iter == 0) || compute_interaction){ for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){ for(unsigned int j=0; j<6; j++){ L[i][j] *= w[i]; } } } if(isoJoIdentity){ LTL = L.AtA(); computeJTR(L, R, LTR); switch(m_optimizationMethod){ case vpMbTracker::LEVENBERG_MARQUARDT_OPT: { vpMatrix LMA(LTL.getRows(), LTL.getCols()); LMA.eye(); vpMatrix LTLmuI = LTL + (LMA*mu); v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR; if(iter != 0) mu /= 10.0; error_prev = m_error; break; } case vpMbTracker::GAUSS_NEWTON_OPT: default: v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR; } } else{ vpVelocityTwistMatrix cVo; cVo.buildFrom(cMo); vpMatrix LVJ = (L*cVo*oJo); vpMatrix LVJTLVJ = (LVJ).AtA(); vpColVector LVJTR; computeJTR(LVJ, R, LVJTR); switch(m_optimizationMethod){ case vpMbTracker::LEVENBERG_MARQUARDT_OPT: { vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols()); LMA.eye(); vpMatrix LTLmuI = LVJTLVJ + (LMA*mu); v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR; v = cVo * v; if(iter != 0) mu /= 10.0; error_prev = m_error; break; } case vpMbTracker::GAUSS_NEWTON_OPT: default: { v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR; v = cVo * v; break; } } } cMoPrev = cMo; ctTc0_Prev = ctTc0; ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0; cMo = ctTc0 * c0Mo; }