def runFilter(self,accMeas,gyroMeas,otherMeas,dT): #spool up after initialization if (self.timeConstant < self.tcTarget) : self.timeConstant += .0001*(self.tcTarget - self.timeConstant) + .001 # integrate angular velocity to get omega = gyroMeas - self.gyroBias; # get the magnitude of the angular velocity magOmega = np.linalg.norm(omega) # recover the axis around which it's spinning axis = omega/(magOmega + 1e-13) # calculate the angle through which it has spun angle = magOmega*dT # make a quaternion deltaAttPredict = AQ.quatFromAxisAngle(axis,angle) # integrate attitude self.attitudeEstimate = deltaAttPredict*self.attitudeEstimate # unpack attitude into Eulers r,p,y = self.attitudeEstimate.asEuler # Do we have magnetometer information? for ii in otherMeas: if (ii[0]=='mag'): # rotate magnetometer reading into earth reference frame magReadingInEarth = np.dot(self.attitudeEstimate.asRotMat.T,ii[1]) # expected magnetometer reading. # Declination can be determined if you know your GPS coordinates. # The code in Arducopter does this. #earthMagReading = np.array([[.48407,.12519,.8660254]]).T # corresponds to 14.5deg east, 60deg downward inclination earthMagReading = ii[2] # corresponds to 14.5deg east, 60deg downward inclination # the z-component of the cross product of these two quantities is proportional to the sine of the yaw error, # for small angles, sin(x) ~= x yawErr = np.cross(magReadingInEarth.T,earthMagReading.T) # update yaw y = y + 180./np.pi*yawErr[0][2] # do first slerp for yaw updateQuat = AQ.Quaternion(np.array([r,p,y])) newAtt = AQ.slerp(self.attitudeEstimate, updateQuat, min(20*dT/(self.timeConstant+dT),.5) ) # process accel data (if magnitude of acceleration is around 1g) r,p,y = newAtt.asEuler if (abs(np.linalg.norm(accMeas) - grav) < .5): # incorporate accelerometer data #updateQuat = attFromAccel(y,accMeas) deltaA = np.cross((newAtt.asRotMat*(-gravity)).T,accMeas.T) dMagAcc = np.linalg.norm(deltaA) if (dMagAcc != 0): axisAcc = (1./dMagAcc)*deltaA else: axisAcc = np.array([[1.,0.,0.]]) updateQuat = AQ.quatFromAxisAngle(axisAcc,dMagAcc) else: # rebuild attitude estimate. May have no change, but 'y' may have magnetometer info incorporated into it, which doesn't care if we're accelerating. updateQuat = AQ.Quaternion(np.array([r,p,y])) # slerp between integrated and measured attitude newAtt = AQ.slerp(newAtt,updateQuat, dT/(self.timeConstant+dT) ) # calculate difference between measurement and integrated for bias calc errorQuat = (newAtt*self.attitudeEstimate.inv()).q # calculate bias derivative # if we think of a quaternion in terms of the axis-angle representation of a rotation, then # dividing the vector component of a quaternion by its scalar component gives a vector proportional # to tan(angle/2). For small angles this is pretty close to angle/2. biasDot = -np.dot(np.array([[self.Kbias,0,0],[0, self.Kbias,0],[0,0,self.Kbias]]),(1./errorQuat[3])*np.array([[errorQuat[0]],[errorQuat[1]],[errorQuat[2]]])) # Update gyro bias self.gyroBias = self.gyroBias + dT*biasDot # update attitude self.attitudeEstimate = newAtt return [self.attitudeEstimate, self.gyroBias]
def runFilter(self, accMeas, gyroMeas, otherMeas, dT): #spool up after initialization if (self.timeConstant < self.tcTarget): self.timeConstant += .0001 * (self.tcTarget - self.timeConstant) + .001 # integrate angular velocity to get omega = gyroMeas - self.gyroBias # get the magnitude of the angular velocity magOmega = np.linalg.norm(omega) # recover the axis around which it's spinning axis = omega / (magOmega + 1e-13) # calculate the angle through which it has spun angle = magOmega * dT # make a quaternion deltaAttPredict = AQ.quatFromAxisAngle(axis, angle) # integrate attitude self.attitudeEstimate = deltaAttPredict * self.attitudeEstimate # unpack attitude into Eulers r, p, y = self.attitudeEstimate.asEuler # Do we have magnetometer information? for ii in otherMeas: if (ii[0] == 'mag'): # rotate magnetometer reading into earth reference frame magReadingInEarth = np.dot(self.attitudeEstimate.asRotMat.T, ii[1]) # expected magnetometer reading. # Declination can be determined if you know your GPS coordinates. # The code in Arducopter does this. #earthMagReading = np.array([[.48407,.12519,.8660254]]).T # corresponds to 14.5deg east, 60deg downward inclination earthMagReading = ii[ 2] # corresponds to 14.5deg east, 60deg downward inclination # the z-component of the cross product of these two quantities is proportional to the sine of the yaw error, # for small angles, sin(x) ~= x yawErr = np.cross(magReadingInEarth.T, earthMagReading.T) # update yaw y = y + 180. / np.pi * yawErr[0][2] # do first slerp for yaw updateQuat = AQ.Quaternion(np.array([r, p, y])) newAtt = AQ.slerp(self.attitudeEstimate, updateQuat, min(20 * dT / (self.timeConstant + dT), .5)) # process accel data (if magnitude of acceleration is around 1g) r, p, y = newAtt.asEuler if (abs(np.linalg.norm(accMeas) - grav) < .5): # incorporate accelerometer data #updateQuat = attFromAccel(y,accMeas) deltaA = np.cross((newAtt.asRotMat * (-gravity)).T, accMeas.T) dMagAcc = np.linalg.norm(deltaA) if (dMagAcc != 0): axisAcc = (1. / dMagAcc) * deltaA else: axisAcc = np.array([[1., 0., 0.]]) updateQuat = AQ.quatFromAxisAngle(axisAcc, dMagAcc) else: # rebuild attitude estimate. May have no change, but 'y' may have magnetometer info incorporated into it, which doesn't care if we're accelerating. updateQuat = AQ.Quaternion(np.array([r, p, y])) # slerp between integrated and measured attitude newAtt = AQ.slerp(newAtt, updateQuat, dT / (self.timeConstant + dT)) # calculate difference between measurement and integrated for bias calc errorQuat = (newAtt * self.attitudeEstimate.inv()).q # calculate bias derivative # if we think of a quaternion in terms of the axis-angle representation of a rotation, then # dividing the vector component of a quaternion by its scalar component gives a vector proportional # to tan(angle/2). For small angles this is pretty close to angle/2. biasDot = -np.dot( np.array([[self.Kbias, 0, 0], [0, self.Kbias, 0], [0, 0, self.Kbias]]), (1. / errorQuat[3]) * np.array([[errorQuat[0]], [errorQuat[1]], [errorQuat[2]]])) # Update gyro bias self.gyroBias = self.gyroBias + dT * biasDot # update attitude self.attitudeEstimate = newAtt return [self.attitudeEstimate, self.gyroBias]