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

It ain't perfect, but I'm going with what I've got for now.

Browse source
This commit is contained in:
Andrzej Kapolka 2013-06-11 13:18:13 -07:00
parent 517bbe80a7
commit 5ba1d3a28c
1 changed files with 5 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

14
interface/src/SerialInterface.cpp
View file

@ -249,14 +249,15 @@ void SerialInterface::readData(float deltaTime) {
_angularAccelToLinearAccel * angularAcceleration; _angularAccelToLinearAccel * angularAcceleration;
glm::vec3 error = _estimatedAcceleration - predictedAcceleration; glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
printLog("error %g\n", glm::length(error)); // the "error" is actually what we want: the linear acceleration minus rotational influences
_estimatedAcceleration = error;
// adjust according to error in each dimension, in proportion to input magnitudes // adjust according to error in each dimension, in proportion to input magnitudes
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (fabsf(error[i]) < EPSILON) { if (fabsf(error[i]) < EPSILON) {
continue; continue;
} }
const float LEARNING_RATE = 0.1f; const float LEARNING_RATE = 0.01f;
float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z); float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
if (rateSum > EPSILON) { if (rateSum > EPSILON) {
for (int j = 0; j < 3; j++) { for (int j = 0; j < 3; j++) {
@ -278,13 +279,8 @@ void SerialInterface::readData(float deltaTime) {
} }
} }
printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][0], _angularVelocityToLinearAccel[1][0], _angularVelocityToLinearAccel[2][0]); // rotate estimated acceleration into global rotation frame
printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][1], _angularVelocityToLinearAccel[1][1], _angularVelocityToLinearAccel[2][1]); _estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
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;