History

Working on "learning" the acceleration matrices.

Andrzej Kapolka committed 13 years ago

Commit 73ceea71 parent 988c4f92 files · +49 −13Browse files

Showing the changes to interface/src/SerialInterface.cpp (1 of 2 files). Show all

Unified · Split

interface/src/SerialInterface.cpp+43−12
@@ -239,19 +239,50 @@ void SerialInterface::readData(float deltaTime) { · 238 unmodified lines
239239 glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
240240 glm::quat(glm::radians(deltaTime * _lastRotationRates));
241241
242− // The acceleration matrix transforms angular to linear accelerations
243− const glm::vec3 PIVOT_OFFSET(0.0f, -0.02f, -0.01f);
244− const glm::vec3 PIVOT_OFFSET_NORMALIZED = glm::normalize(PIVOT_OFFSET);
245− const glm::vec3 PIVOT_SINES = glm::max(glm::vec3(EPSILON, EPSILON, EPSILON),
246− glm::sqrt(glm::vec3(1.0f, 1.0f, 1.0f) - PIVOT_OFFSET_NORMALIZED * PIVOT_OFFSET_NORMALIZED));
247− const glm::mat3 ACCELERATION_MATRIX(
248− 0.0f, PIVOT_OFFSET.z / PIVOT_SINES.x, -PIVOT_OFFSET.y / PIVOT_SINES.x,
249− -PIVOT_OFFSET.z / PIVOT_SINES.y, 0.0f, PIVOT_OFFSET.x / PIVOT_SINES.y,
250− PIVOT_OFFSET.y / PIVOT_SINES.z, -PIVOT_OFFSET.x / PIVOT_SINES.z, 0.0f);
242+ // Update acceleration estimate: first, subtract gravity as rotated into current frame
243+ _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
244+
245+ // Consider updating our angular velocity/acceleration to linear acceleration mapping
246+ if (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) {
247+ // compute predicted linear acceleration, find error between actual and predicted
248+ glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
249+ _angularAccelToLinearAccel * angularAcceleration;
250+ glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
251251
252− // Update acceleration estimate
253− _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity -
254− ACCELERATION_MATRIX * angularAcceleration;
252+ // adjust according to error in each dimension, in proportion to input magnitudes
253+ for (int i = 0; i < 3; i++) {
254+ if (fabsf(error[i]) < EPSILON) {
255+ continue;
256+ }
257+ const float LEARNING_RATE = 0.1f;
258+ float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
259+ if (rateSum > EPSILON) {
260+ for (int j = 0; j < 3; j++) {
261+ float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
262+ if (proportion > EPSILON) {
263+ _angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
264+ }
265+ }
266+ }
267+ float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
268+ if (accelSum > EPSILON) {
269+ for (int j = 0; j < 3; j++) {
270+ float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
271+ if (proportion > EPSILON) {
272+ _angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
273+ }
274+ }
275+ }
276+ }
277+ }
278+
279+ printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][0], _angularVelocityToLinearAccel[1][0], _angularVelocityToLinearAccel[2][0]);
280+ printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][1], _angularVelocityToLinearAccel[1][1], _angularVelocityToLinearAccel[2][1]);
281+ printLog("%g %g %g\n\n", _angularVelocityToLinearAccel[0][2], _angularVelocityToLinearAccel[1][2], _angularVelocityToLinearAccel[2][2]);
282+
283+ printLog("%g %g %g\n", _angularAccelToLinearAccel[0][0], _angularAccelToLinearAccel[1][0], _angularAccelToLinearAccel[2][0]);
284+ printLog("%g %g %g\n", _angularAccelToLinearAccel[0][1], _angularAccelToLinearAccel[1][1], _angularAccelToLinearAccel[2][1]);
285+ printLog("%g %g %g\n\n", _angularAccelToLinearAccel[0][2], _angularAccelToLinearAccel[1][2], _angularAccelToLinearAccel[2][2]);
255286
256287 // Update estimated position and velocity
257288 float const DECAY_VELOCITY = 0.95f;