public void HandleIMUData(double yawDown, double pitchLeft, double rollLeft, double accX, double accY, double accZ)
{
integrator.Update(new [] { yawDown, pitchLeft, rollLeft });
FusedValues = new EulerAngles(integrator.Values[0], integrator.Values[1], integrator.Values[2]);
}
public SimpleIntegrationMotionPlusFuser()
{
integrator = new Integrator(3);
FusedValues = new EulerAngles(0, 0, 0);
}