void KalmanFilter<DataTypes, DepthTypes>::estimate(vpColVector& f,vpColVector& p) { K = PPred*H.transpose()*((H*PPred*H.transpose() + R).pseudoInverse()); //Kv = PvPred*((PvPred + R).pseudoInverse()); //Kv = PvPred*((PvPred + covMes).pseudoInverse()); std::cout << " Kalman gain " << K << std::endl; //vEst = vPred + (Kv)*(vMes-vPred); measuredState = p; measuredState.stack(f); estimatedState = predictedState + K*(measuredState - predictedState); PEst = (I-K*H)*PPred; estimatedPositions = estimatedState.rows(0, f.getCols()-1); estimatedForces = estimatedState.rows(f.getCols(), 2*f.getCols()-1); //PvEst = (Iv-Kv)*PvPred; }
void KalmanFilter<DataTypes, DepthTypes>::init(vpColVector& f,vpColVector& p, vpMatrix& stiffnessMatrix) { H.resize(2*f.getCols(),2*f.getCols()); H.setIdentity(); J.resize(2*f.getCols(),2*f.getCols()); J.setIdentity(); for (int k = f.getCols(); k < 2*f.getCols(); k++) for (int l = f.getCols(); l < 2*f.getCols(); l++) J[k][l] = stiffnessMatrix[k-f.getCols()][l-f.getCols()]; Q.resize(2*f.getCols(),2*f.getCols()); R.resize(2*f.getCols(),2*f.getCols()); PEst.resize(2*f.getCols(),2*f.getCols()); I.resize(2*f.getCols(),2*f.getCols()); I.setIdentity(); PEst = R; qF = 1; rF = 0.2; qX = 0.01; rX = 0.005; for (int k = 0; k < f.getCols(); k++) { Q[k][k] = (double)qX.getValue(); Q[k+f.getCols()][k+f.getCols()] = (double)qF.getValue(); R[k][k] = (double)rX.getValue(); R[k+f.getCols()][k+f.getCols()] = (double)rF.getValue(); } estimatedForces = f; estimatedPositions = p; estimatedState = estimatedPositions; estimatedState.stack(estimatedForces); }