예제 #1
0
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;	
	
}
예제 #2
0
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);	
}