Showing the changes to interface/src/SerialInterface.cpp (1 of 2 files). Show all
Unified · Split
| @@ -239,19 +239,50 @@ void SerialInterface::readData(float deltaTime) { · 238 unmodified lines | |||
| 239 | glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) * | 239 | glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) * |
| 240 | glm::quat(glm::radians(deltaTime * _lastRotationRates)); | 240 | glm::quat(glm::radians(deltaTime * _lastRotationRates)); |
| 241 | 241 | ||
| 242 | // The acceleration matrix transforms angular to linear accelerations | 242 | // Update acceleration estimate: first, subtract gravity as rotated into current frame |
| 243 | const glm::vec3 PIVOT_OFFSET(0.0f, -0.02f, -0.01f); | 243 | _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity; |
| 244 | const glm::vec3 PIVOT_OFFSET_NORMALIZED = glm::normalize(PIVOT_OFFSET); | 244 | |
| 245 | const glm::vec3 PIVOT_SINES = glm::max(glm::vec3(EPSILON, EPSILON, EPSILON), | 245 | // Consider updating our angular velocity/acceleration to linear acceleration mapping |
| 246 | glm::sqrt(glm::vec3(1.0f, 1.0f, 1.0f) - PIVOT_OFFSET_NORMALIZED * PIVOT_OFFSET_NORMALIZED)); | 246 | if (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) { |
| 247 | const glm::mat3 ACCELERATION_MATRIX( | 247 | // compute predicted linear acceleration, find error between actual and predicted |
| 248 | 0.0f, PIVOT_OFFSET.z / PIVOT_SINES.x, -PIVOT_OFFSET.y / PIVOT_SINES.x, | 248 | glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates + |
| 249 | -PIVOT_OFFSET.z / PIVOT_SINES.y, 0.0f, PIVOT_OFFSET.x / PIVOT_SINES.y, | 249 | _angularAccelToLinearAccel * angularAcceleration; |
| 250 | PIVOT_OFFSET.y / PIVOT_SINES.z, -PIVOT_OFFSET.x / PIVOT_SINES.z, 0.0f); | 250 | glm::vec3 error = _estimatedAcceleration - predictedAcceleration; |
| 251 | 251 | ||
| 252 | // Update acceleration estimate | 252 | // adjust according to error in each dimension, in proportion to input magnitudes |
| 253 | _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity - | 253 | for (int i = 0; i < 3; i++) { |
| 254 | ACCELERATION_MATRIX * angularAcceleration; | 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]); | ||
| 255 | 286 | ||
| 256 | // Update estimated position and velocity | 287 | // Update estimated position and velocity |
| 257 | float const DECAY_VELOCITY = 0.95f; | 288 | float const DECAY_VELOCITY = 0.95f; |