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]
