Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-06 10:43:45 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Working on "learning" the acceleration matrices.

Browse source
This commit is contained in:
Andrzej Kapolka 2013-06-11 10:55:28 -07:00
parent 988c4f97b0
commit 73ceea7795
2 changed files with 49 additions and 13 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

55
interface/src/SerialInterface.cpp
View file

@ -239,19 +239,50 @@ void SerialInterface::readData(float deltaTime) {
glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) * glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
glm::quat(glm::radians(deltaTime * _lastRotationRates)); glm::quat(glm::radians(deltaTime * _lastRotationRates));
// The acceleration matrix transforms angular to linear accelerations // Update acceleration estimate: first, subtract gravity as rotated into current frame
const glm::vec3 PIVOT_OFFSET(0.0f, -0.02f, -0.01f); _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
const glm::vec3 PIVOT_OFFSET_NORMALIZED = glm::normalize(PIVOT_OFFSET);
const glm::vec3 PIVOT_SINES = glm::max(glm::vec3(EPSILON, EPSILON, EPSILON), // Consider updating our angular velocity/acceleration to linear acceleration mapping
glm::sqrt(glm::vec3(1.0f, 1.0f, 1.0f) - PIVOT_OFFSET_NORMALIZED * PIVOT_OFFSET_NORMALIZED)); if (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) {
const glm::mat3 ACCELERATION_MATRIX( // compute predicted linear acceleration, find error between actual and predicted
0.0f, PIVOT_OFFSET.z / PIVOT_SINES.x, -PIVOT_OFFSET.y / PIVOT_SINES.x, glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
-PIVOT_OFFSET.z / PIVOT_SINES.y, 0.0f, PIVOT_OFFSET.x / PIVOT_SINES.y, _angularAccelToLinearAccel * angularAcceleration;
PIVOT_OFFSET.y / PIVOT_SINES.z, -PIVOT_OFFSET.x / PIVOT_SINES.z, 0.0f); glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
// Update acceleration estimate // adjust according to error in each dimension, in proportion to input magnitudes
_estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity - for (int i = 0; i < 3; i++) {
ACCELERATION_MATRIX * angularAcceleration; if (fabsf(error[i]) < EPSILON) {
continue;
}
const float LEARNING_RATE = 0.1f;
float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
if (rateSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
if (proportion > EPSILON) {
_angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
}
}
}
float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
if (accelSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
if (proportion > EPSILON) {
_angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
}
}
}
}
}
printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][0], _angularVelocityToLinearAccel[1][0], _angularVelocityToLinearAccel[2][0]);
printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][1], _angularVelocityToLinearAccel[1][1], _angularVelocityToLinearAccel[2][1]);
printLog("%g %g %g\n\n", _angularVelocityToLinearAccel[0][2], _angularVelocityToLinearAccel[1][2], _angularVelocityToLinearAccel[2][2]);
printLog("%g %g %g\n", _angularAccelToLinearAccel[0][0], _angularAccelToLinearAccel[1][0], _angularAccelToLinearAccel[2][0]);
printLog("%g %g %g\n", _angularAccelToLinearAccel[0][1], _angularAccelToLinearAccel[1][1], _angularAccelToLinearAccel[2][1]);
printLog("%g %g %g\n\n", _angularAccelToLinearAccel[0][2], _angularAccelToLinearAccel[1][2], _angularAccelToLinearAccel[2][2]);
// Update estimated position and velocity // Update estimated position and velocity
float const DECAY_VELOCITY = 0.95f; float const DECAY_VELOCITY = 0.95f;

7
interface/src/SerialInterface.h
View file

@ -30,7 +30,9 @@ public:
_estimatedPosition(0, 0, 0), _estimatedPosition(0, 0, 0),
_estimatedVelocity(0, 0, 0), _estimatedVelocity(0, 0, 0),
_lastAcceleration(0, 0, 0), _lastAcceleration(0, 0, 0),
_lastRotationRates(0, 0, 0) _lastRotationRates(0, 0, 0),
_angularVelocityToLinearAccel(0),
_angularAccelToLinearAccel(0)
{} {}
void pair(); void pair();
@ -64,6 +66,9 @@ private:
glm::vec3 _estimatedAcceleration; glm::vec3 _estimatedAcceleration;
glm::vec3 _lastAcceleration; glm::vec3 _lastAcceleration;
glm::vec3 _lastRotationRates; glm::vec3 _lastRotationRates;
glm::mat3 _angularVelocityToLinearAccel;
glm::mat3 _angularAccelToLinearAccel;
}; };
#endif #endif