1
Ich arbeite gerade an iOS Projekt, wo ich Bewegungsdaten verwende. Ich bekomme gute Ergebnisse mit Pitch-and-Roll-Werten, aber der Gierwert driftet ständig. Ich habe Kalman Filter angewendet und die Ergebnisse sind gleich geblieben. Hat jemand eine Idee, wie man es löst? Hier einige Quellcode (Objective C)Yaw (Winkel) Wert sind nicht stabil, es ist ein paar Grad treiben. Weiß jemand, wie man es löst?
[self.motionManager startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXArbitraryCorrectedZVertical
toQueue:[NSOperationQueue currentQueue]
withHandler:^(CMDeviceMotion *motion, NSError *error)
{
//NSString *yaw = [NSString
//stringWithFormat:@" %.3f", motion.attitude.yaw];
NSString *pitch = [NSString
stringWithFormat:@" %.3f", motion.attitude.pitch];
NSString *roll = [NSString
stringWithFormat:@" %.3f", motion.attitude.roll];
//Converting NSSring type variable in to a double
//double a_yaw = [yaw doubleValue];
double a_pitch = [pitch doubleValue];
double a_roll = [roll doubleValue];
CMQuaternion quat = self.motionManager.deviceMotion.attitude.quaternion;
double yaw = 180/M_PI * (asin(2*quat.x*quat.y + 2*quat.w*quat.z));
// Kalman filtering
static float q = 0.1; // process noise
static float r = 0.1; // sensor noise
static float p = 0.1; // estimated error
static float k = 0.5; // kalman filter gain
float x = motionLastYaw;
p = p + q;
k = p/(p + r);
x = x + k*(yaw - x);
p = (1 - k)*p;
motionLastYaw = x;
//Converting angles to degrees
//yaw = yaw * 180/M_PI;
a_pitch = a_pitch * 180/M_PI;
a_roll = a_roll * 180/M_PI;