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;
}
