//public void SetDelegateMessageHandler(MessageHandler* handler) //{ pDelegate = handler; } // Internal handler for messages; bypasses error checking. private void handleMessage(MessageBodyFrame msg) { // Put the sensor readings into convenient local variables Vector3f gyro = msg.RotationRate; Vector3f accel = msg.Acceleration; Vector3f mag = msg.MagneticField; // Insert current sensor data into filter history FRawMag.AddElement(mag); FAngV.AddElement(gyro); // Apply the calibration parameters to raw mag Vector3f calMag = MagCalibrated ? GetCalibratedMagValue(FRawMag.Mean()) : FRawMag.Mean(); // Set variables accessible through the class API DeltaT = msg.TimeDelta; AngV = gyro; A = accel; RawMag = mag; CalMag = calMag; // Keep track of time Stage++; RunningTime += DeltaT; // Small preprocessing Quatf Qinv = Q.Inverted(); Vector3f up = Qinv.Rotate(new Vector3f(0, 1, 0)); Vector3f gyroCorrected = gyro; // Apply integral term // All the corrections are stored in the Simultaneous Orthogonal Rotations Angle representation, // which allows to combine and scale them by just addition and multiplication if (EnableGravity || EnableYawCorrection) gyroCorrected -= GyroOffset; if (EnableGravity) { const float spikeThreshold = 0.01f; const float gravityThreshold = 0.1f; float proportionalGain = 5 * Gain; // Gain parameter should be removed in a future release float integralGain = 0.0125f; Vector3f tiltCorrection = SensorFusion_ComputeCorrection(accel, up); if (Stage > 5) { // Spike detection float tiltAngle = up.Angle(accel); TiltAngleFilter.AddElement(tiltAngle); if (tiltAngle > TiltAngleFilter.Mean() + spikeThreshold) proportionalGain = integralGain = 0; // Acceleration detection const float gravity = 9.8f; if (System.Math.Abs(accel.Length() / gravity - 1) > gravityThreshold) integralGain = 0; } else // Apply full correction at the startup { proportionalGain = 1 / DeltaT; integralGain = 0; } gyroCorrected += (tiltCorrection * proportionalGain); GyroOffset -= (tiltCorrection * integralGain * DeltaT); } if (EnableYawCorrection && MagCalibrated && RunningTime > 2.0f) { const float maxMagRefDist = 0.1f; const float maxTiltError = 0.05f; float proportionalGain = 0.01f; float integralGain = 0.0005f; // Update the reference point if needed if (MagRefIdx < 0 || calMag.Distance(MagRefsInBodyFrame[MagRefIdx]) > maxMagRefDist) { // Delete a bad point if (MagRefIdx >= 0 && MagRefScore < 0) { MagNumReferences--; MagRefsInBodyFrame[MagRefIdx] = MagRefsInBodyFrame[MagNumReferences]; MagRefsInWorldFrame[MagRefIdx] = MagRefsInWorldFrame[MagNumReferences]; } // Find a new one MagRefIdx = -1; MagRefScore = 1000; float bestDist = maxMagRefDist; for (int i = 0; i < MagNumReferences; i++) { float dist = calMag.Distance(MagRefsInBodyFrame[i]); if (bestDist > dist) { bestDist = dist; MagRefIdx = i; } } // Create one if needed if (MagRefIdx < 0 && MagNumReferences < MagMaxReferences) { MagRefIdx = MagNumReferences; MagRefsInBodyFrame[MagRefIdx] = calMag; MagRefsInWorldFrame[MagRefIdx] = Q.Rotate(calMag).Normalized(); MagNumReferences++; } } if (MagRefIdx >= 0) { Vector3f magEstimated = Qinv.Rotate(MagRefsInWorldFrame[MagRefIdx]); Vector3f magMeasured = calMag.Normalized(); // Correction is computed in the horizontal plane (in the world frame) Vector3f yawCorrection = SensorFusion_ComputeCorrection(magMeasured.ProjectToPlane(up), magEstimated.ProjectToPlane(up)); if (System.Math.Abs(up.Dot(magEstimated - magMeasured)) < maxTiltError) { MagRefScore += 2; } else // If the vertical angle is wrong, decrease the score { MagRefScore -= 1; proportionalGain = integralGain = 0; } gyroCorrected += (yawCorrection * proportionalGain); GyroOffset -= (yawCorrection * integralGain * DeltaT); } } // Update the orientation quaternion based on the corrected angular velocity vector float angle = gyroCorrected.Length() * DeltaT; if (angle > 0.0f) Q = Q * new Quatf(gyroCorrected, angle); // The quaternion magnitude may slowly drift due to numerical error, // so it is periodically normalized. if (Stage % 500 == 0) Q.Normalize(); }
public float DistanceSq(Quatf q) { float d1 = (this - q).LengthSq(); float d2 = (this + q).LengthSq(); // Antipodal point check return (d1 < d2) ? d1 : d2; }
// Resets the current orientation. public void Reset() { Q = new Quatf(); QUncorrected = new Quatf(); Stage = 0; RunningTime = 0; MagNumReferences = 0; MagRefIdx = -1; GyroOffset = new Vector3f(); }