void KalmanFilter::updateWithObservation(messages::VBall visionBall)
{
// Declare C and C transpose (ublas)
// C takes state estimate to observation frame so
// c = 1 0 0 0
// 0 1 0 0
ufmatrix c (2,4);
for(unsigned i=0; i<c.size1(); i++){
for(unsigned j=0; j<c.size2(); j++){
if(i == j)
c(i,j) = 1.f;
else
c(i,j) = 0.f;
}
}
ufmatrix cTranspose(4,2);
cTranspose = trans(c);
// Calculate the gain
// Calc c*cov*c^t
ufmatrix cCovCTranspose(2,2);
cCovCTranspose = prod(cov,cTranspose);
cCovCTranspose = prod(c,cCovCTranspose);
cCovCTranspose(0,0) += params.obsvRelXVariance;
cCovCTranspose(1,1) += params.obsvRelYVariance;
// gain = cov*c^t*(c*cov*c^t + var)^-1
ufmatrix kalmanGain(2,2);
kalmanGain = prod(cTranspose,NBMath::invert2by2(cCovCTranspose));
kalmanGain = prod(cov,kalmanGain);
ufvector posEstimates(2);
posEstimates = prod(c, x);
// x straight ahead, y to the right
float sinB,cosB;
sincosf(visionBall.bearing(),&sinB,&cosB);
ufvector measurement(2);
measurement(0) = visionBall.distance()*cosB;
measurement(1) = visionBall.distance()*sinB;
ufvector innovation(2);
innovation = measurement - posEstimates;
ufvector correction(4);
correction = prod(kalmanGain,innovation);
x += correction;
//cov = cov - k*c*cov
ufmatrix4 identity;
identity = boost::numeric::ublas::identity_matrix <float>(4);
ufmatrix4 modifyCov;
modifyCov = identity - prod(kalmanGain,c);
cov = prod(modifyCov,cov);
// Housekeep
filteredDist = std::sqrt(x(0)*x(0) + x(1)*x(1));
filteredBear = NBMath::safe_atan2(x(1),x(0));
float curErr = std::sqrt(correction(0)*correction(0) + correction(1)*correction(1));
correctionMagBuffer[curEntry] = curErr;
score = 0.f;
for(int i=0; i<BUFF_LENGTH; i++)
score+= correctionMagBuffer[i];
score = score/10; // avg correction over 10 frames
//get magnitude of correction
// std::cout << "Is moving filter " << isStationary() << std::endl;
// std::cout << "Score: " << score << " cur error " << curErr << std::endl;
}
void updateHaptics(void)
{
// start haptic device
hapticDevice->open();
// simulation clock
cPrecisionClock simClock;
simClock.start(true);
double kp, kr;
kp = 0.0;
kr = 0.0;
cMatrix3d prevRotTool;
prevRotTool.identity();
// main haptic simulation loop
while(simulationRunning)
{
// retrieve simulation time and compute next interval
double time = simClock.getCurrentTimeSeconds();
double nextSimInterval = simClock.stop();//0.0005;//cClamp(time, 0.00001, 0.0002);
// reset clock
simClock.reset();
simClock.start();
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
cVector3d posDevice;
cMatrix3d rotDevice;
hapticDevice->getPosition(posDevice);
hapticDevice->getRotation(rotDevice);
// scale position of device
posDevice.mul(workspaceScaleFactor);
// read position of tool
cVector3d posTool = ODETool->getLocalPos();
cMatrix3d rotTool = ODETool->getLocalRot();
// compute position and angular error between tool and haptic device
cVector3d deltaPos = (posDevice - posTool);
cMatrix3d deltaRot = cMul(cTranspose(rotTool), rotDevice);
double angle;
cVector3d axis;
deltaRot.toAngleAxis(angle, axis);
// compute force and torque to apply to tool
cVector3d force, torque;
force = linStiffness * deltaPos;
ODETool->addExternalForce(force);
torque = cMul((angStiffness * angle), axis);
rotTool.mul(torque);
ODETool->addExternalTorque(torque);
// update data for display
frame->setLocalRot(deltaRot);
// compute force and torque to apply to haptic device
force = -linG * force;
torque = -angG * torque;
line->m_pointB = torque;
// send forces to device
hapticDevice->setForceAndTorqueAndGripperForce(force, torque, 0.0);
if (linG < linGain)
{
linG = linG + 0.1 * time * linGain;
}
else
{
linG = linGain;
}
if (angG < angGain)
{
angG = angG + 0.1 * time * angGain;
}
else
{
angG = angGain;
}
// update simulation
ODEWorld->updateDynamics(nextSimInterval);
}
// exit haptics thread
simulationFinished = true;
}
