void UKFSample::poseSensorUpdate(const Vector3f& reading, const Matrix3x3f& readingCov)
{
generateSigmaPoints();
// computePoseReadings
Vector3f poseReadings[7];
for(int i = 0; i < 7; ++i)
poseReadings[i] = Vector3f(sigmaPoints[i].x, sigmaPoints[i].y, sigmaPoints[i].z);
// computeMeanOfPoseReadings
Vector3f poseReadingMean = poseReadings[0];
for(int i = 1; i < 7; ++i)
poseReadingMean += poseReadings[i];
poseReadingMean *= 1.f / 7.f;
// computeCovOfPoseReadingsAndSigmaPoints
Matrix3x3f poseReadingAndMeanCov;
for(int i = 0; i < 3; ++i)
{
Vector3f d1 = poseReadings[i * 2 + 1] - poseReadingMean;
poseReadingAndMeanCov += Matrix3x3f(d1 * l[i].x, d1 * l[i].y, d1 * l[i].z);
Vector3f d2 = poseReadings[i * 2 + 2] - poseReadingMean;
poseReadingAndMeanCov += Matrix3x3f(d2 * -l[i].x, d2 * -l[i].y, d2 * -l[i].z);
}
poseReadingAndMeanCov *= 0.5f;
// computeCovOfPoseReadingsReadings
Matrix3x3f poseReadingCov;
Vector3f d = poseReadings[0] - poseReadingMean;
poseReadingCov = Matrix3x3f(d * d.x, d * d.y, d * d.z);
for(int i = 1; i < 7; ++i)
{
Vector3f d = poseReadings[i] - poseReadingMean;
poseReadingCov += Matrix3x3f(d * d.x, d * d.y, d * d.z);
}
poseReadingCov *= 0.5f;
poseReadingMean.z = normalize(poseReadingMean.z);
const Matrix3x3f kalmanGain = poseReadingAndMeanCov.transpose() * (poseReadingCov + readingCov).invert();
Vector3f innovation = reading - poseReadingMean;
innovation.z = normalize(innovation.z);
const Vector3f correction = kalmanGain * innovation;
mean += correction;
mean.z = normalize(mean.z);
cov -= kalmanGain * poseReadingAndMeanCov;
}
void FlyingLayer::Update(TimeDelta real_time_delta) {
static const float PERIOD_TRANS = 0.0045f;
static const float PERIOD_ROT = 1.0f;
static const float FILTER = 0.2f;
if (m_Palms.size() > 0) {
Vector3f positionSum = Vector3f::Zero();
Vector3f rotationAASum = Vector3f::Zero();
for (int i = 0; i < m_Palms.size(); i++) {
positionSum += m_GridOrientation.block<3, 3>(0, 0).transpose()*(m_Palms[i] - m_EyePos - m_EyeView.transpose()*Vector3f(0, -0.15, -0.05));
//rotationAASum += RotationMatrixToVector(m_EyeView.transpose()*m_PalmOrientations[i]*m_EyeView.transpose());
Matrix3x3f rot;
RotationMatrix_VectorToVector(-Vector3f::UnitZ(), m_EyeView*(m_Palms[i] - m_EyePos) - Vector3f(0, -0.15, -0.05), rot);
//std::cout << __LINE__ << ":\t rot = " << (rot) << std::endl;
rotationAASum += RotationMatrixToVector(rot);
}
if (m_Palms.size() == 2) {
const Vector3f dir0 = m_EyeView*(m_Palms[0] - m_EyePos) - Vector3f(0, -0.15, -0.05);
const Vector3f dir1 = m_EyeView*(m_Palms[1] - m_EyePos) - Vector3f(0, -0.15, -0.05);
Matrix3x3f rot;
RotationMatrix_VectorToVector((dir0.x() < dir1.x() ? 1.0f : -1.0f) * Vector3f::UnitX(), dir1 - dir0, rot);
//std::cout << __LINE__ << ":\t positionSum = " << (positionSum) << std::endl;
rotationAASum += 2.0f*RotationMatrixToVector(rot);
}
m_Velocity = (1 - FILTER)*m_Velocity + FILTER*positionSum/m_Palms.size();
m_RotationAA = (1 - FILTER)*m_RotationAA + FILTER*rotationAASum/m_Palms.size();
} else {
m_Velocity = (1 - 0.3f*FILTER)*m_Velocity;
m_RotationAA = (1 - 0.3f*FILTER)*m_RotationAA;
}
m_GridCenter -= m_Velocity*m_Velocity.squaredNorm()*(real_time_delta/PERIOD_TRANS);
const Matrix3x3f rot = RotationVectorToMatrix((real_time_delta/PERIOD_ROT)*m_RotationAA*m_RotationAA.squaredNorm());
//std::cout << __LINE__ << ":\t rot = " << (rot) << std::endl;
//Matrix3x3f foo = ;
m_GridOrientation.block<3, 3>(0, 0) = m_EyeView.transpose()*rot.transpose()*m_EyeView*m_GridOrientation.block<3, 3>(0, 0);
}
Matrix3x3f FlyingLayer::RotationMatrixLinearInterpolation(const Matrix3x3f& A0, const Matrix3x3f& A1, float t) const {
Vector3f dA = RotationMatrixToVector(A0.transpose()*A1);
float angle = std::fmod(t*dA.norm(), M_PI);
Matrix3x3f At = A0*RotationVectorToMatrix(angle*dA.normalized());
return At;
}