def magUpdate(self, attHat, magMeas, alpha, worldMag):
'''
expectedMag = np.dot(attHat.asRotMat,worldMag)
axisangle = np.cross(magMeas.T,expectedMag.T).T
axis = 1./((np.linalg.norm(axisangle))+1e-5)*axisangle
angle = np.arcsin(np.linalg.norm(axisangle))
#print axis.T, angle
#print ' '
#qMag = quatFromRotVec(alpha*axisangle)
qMag = AQ.quatFromAxisAngle(axis,alpha*angle)
'''
# Code for using mag ONLY for yaw
# rotate mag reading into world
magInWorld = np.dot(attHat.asRotMat.T, magMeas)
magInWorld[2][0] = 0.0
magInWorld = 1 / (np.linalg.norm(magInWorld)) * magInWorld
worldMag[2][0] = 0.0
worldMag = 1 / (np.linalg.norm(worldMag)) * worldMag
axisSinAngle = np.cross(magInWorld.T, worldMag.T).T
angle = np.arcsin(np.linalg.norm(axisSinAngle))
axis = (1. / (np.linalg.norm(axisSinAngle) + 1e-5)) * axisSinAngle
#print axis.T,angle
#axis = np.dot(attHat.asRotMat,axis)
qMag = AQ.quatFromAxisAngle(axis, angle * alpha)
return attHat * qMag #qMag*attHat #AQ.slerp(attHat,qMag,alpha)
def magUpdate(self,attHat,magMeas,alpha,worldMag):
'''
expectedMag = np.dot(attHat.asRotMat,worldMag)
axisangle = np.cross(magMeas.T,expectedMag.T).T
axis = 1./((np.linalg.norm(axisangle))+1e-5)*axisangle
angle = np.arcsin(np.linalg.norm(axisangle))
#print axis.T, angle
#print ' '
#qMag = quatFromRotVec(alpha*axisangle)
qMag = AQ.quatFromAxisAngle(axis,alpha*angle)
'''
# Code for using mag ONLY for yaw
# rotate mag reading into world
magInWorld = np.dot(attHat.asRotMat.T,magMeas)
magInWorld[2][0] = 0.0
magInWorld = 1/(np.linalg.norm(magInWorld))*magInWorld
worldMag[2][0] = 0.0
worldMag = 1/(np.linalg.norm(worldMag))*worldMag
axisSinAngle = np.cross(magInWorld.T,worldMag.T).T
angle = np.arcsin(np.linalg.norm(axisSinAngle))
axis = (1./(np.linalg.norm(axisSinAngle) + 1e-5))*axisSinAngle
#print axis.T,angle
#axis = np.dot(attHat.asRotMat,axis)
qMag = AQ.quatFromAxisAngle(axis,angle*alpha)
return attHat*qMag #qMag*attHat #AQ.slerp(attHat,qMag,alpha)
def testHarness():
testEKF = True
if (testEKF):
filt = EKF()
else:
filt = KF()
accel_world = np.array([[0.1,.0,0]]).T
gravMeas = np.array([[.0,.0,-9.81]]).T
gyro = np.array([[.0,.0,.1]]).T
omega = np.linalg.norm(gyro)
velocity = np.zeros((3,1))
pos = velocity
dT = 0.01
att = AQ.Quaternion([0,0,0])
for ii in range(0,1000):
#att = AQ.quatFromAxisAngle(gyro,np.linalg.norm(gyro)*dT)*att
accelerometer = np.dot(att.asRotMat,accel_world+gravMeas)
att = AQ.quatFromAxisAngle(gyro,np.linalg.norm(gyro)*dT)*att
if (ii%21 == 0):
gps = ['gps',pos,.01*np.eye(3)]
other = [gps]
else:
other = [['g',omega,np.eye(3)]]
if (testEKF):
stateAndCov = filt.runFilter(accelerometer+1.*np.random.randn(3,1),gyro,other,dT)
else:
stateAndCov = filt.runFilter(accel_world+.01*np.random.randn(3,1),other,dT)
if 'states' in locals():
states = np.hstack((states,stateAndCov[0]))
covs = np.vstack((covs,np.diag(stateAndCov[1])))
else:
states = stateAndCov[0]
covs = np.diag(stateAndCov[1])
pos = pos + velocity*dT + .5*dT*dT*accel_world
velocity = velocity + (accel_world)*dT
handles = plt.plot(states.T[:,0:3],marker='s')
plt.legend(['x','y','z'])
plt.figure()
handles = plt.plot(states.T[:,3:6])
plt.legend(['vx','vy','vz'])
plt.figure()
if (testEKF):
handles = plt.plot(states.T[:,6:10])
plt.legend(['qx','qy','qz','qw'])
plt.figure()
plt.plot(covs[:,0:6])
plt.legend(['x','y','z','vx','vy','vz','qx','qy','qz','qw'])
else:
plt.plot(covs[:,0:6])
plt.legend(['x','y','z','vx','vy','vz'])
plt.show()
def testHarness():
testEKF = True
if (testEKF):
filt = EKF()
else:
filt = KF()
accel_world = np.array([[0.1,.0,0]]).T
gravMeas = np.array([[.0,.0,-9.81]]).T
gyro = np.array([[.0,.0,.1]]).T
omega = np.linalg.norm(gyro)
velocity = np.zeros((3,1))
pos = velocity
dT = 0.01
att = AQ.Quaternion([0,0,0])
for ii in range(0,1000):
#att = AQ.quatFromAxisAngle(gyro,np.linalg.norm(gyro)*dT)*att
accelerometer = np.dot(att.asRotMat,accel_world+gravMeas)
att = AQ.quatFromAxisAngle(gyro,np.linalg.norm(gyro)*dT)*att
if (ii%21 == 0):
gps = ['gps',pos,.01*np.eye(3)]
other = [gps]
else:
other = [['g',omega,np.eye(3)]]
if (testEKF):
stateAndCov = filt.runFilter(accelerometer+1.*np.random.randn(3,1),gyro,other,dT)
else:
stateAndCov = filt.runFilter(accel_world+.01*np.random.randn(3,1),other,dT)
if 'states' in locals():
states = np.hstack((states,stateAndCov[0]))
covs = np.vstack((covs,np.diag(stateAndCov[1])))
else:
states = stateAndCov[0]
covs = np.diag(stateAndCov[1])
pos = pos + velocity*dT + .5*dT*dT*accel_world
velocity = velocity + (accel_world)*dT
handles = plt.plot(states.T[:,0:3],marker='s')
plt.legend(['x','y','z'])
plt.figure()
handles = plt.plot(states.T[:,3:6])
plt.legend(['vx','vy','vz'])
plt.figure()
if (testEKF):
handles = plt.plot(states.T[:,6:10])
plt.legend(['qx','qy','qz','qw'])
plt.figure()
plt.plot(covs[:,0:6])
plt.legend(['x','y','z','vx','vy','vz','qx','qy','qz','qw'])
else:
plt.plot(covs[:,0:6])
plt.legend(['x','y','z','vx','vy','vz'])
plt.show()
def accelUpdate(self,attHat,aMeas,alpha, accThresh = .1):
if (np.abs(np.linalg.norm(aMeas) - 9.81) > accThresh):
return attHat
# if measuring 1 g
aMeasNorm = aMeas.T/np.linalg.norm(aMeas)
gBody = np.dot(attHat.asRotMat,-gravdir)
axisangle = np.cross(aMeasNorm,gBody.T).T
angle = np.arcsin(np.linalg.norm(axisangle))
axis = (1./(np.linalg.norm(axisangle)+1e-5))*axisangle
#qAcc = quatFromRotVec(alpha*axisangle)
qAcc = AQ.quatFromAxisAngle(axis,alpha*angle)
# rotate between integrated and measured attitude
return qAcc*attHat
def accelUpdate(self, attHat, aMeas, alpha, accThresh=.1):
if (np.abs(np.linalg.norm(aMeas) - 9.81) > accThresh):
return attHat
# if measuring 1 g
aMeasNorm = aMeas.T / np.linalg.norm(aMeas)
gBody = np.dot(attHat.asRotMat, -gravdir)
axisangle = np.cross(aMeasNorm, gBody.T).T
angle = np.arcsin(np.linalg.norm(axisangle))
axis = (1. / (np.linalg.norm(axisangle) + 1e-5)) * axisangle
#qAcc = quatFromRotVec(alpha*axisangle)
qAcc = AQ.quatFromAxisAngle(axis, alpha * angle)
# rotate between integrated and measured attitude
return qAcc * attHat
def _resetQuat(self):
# In this step we push any mounting error calculated by the measurement update step
# onto the (non-stochastic) reference quaternion carried by the filter.
# Unpack Gibbs vector from state
errorAngle = np.linalg.norm(self.state[6:9])
# Gibbs vector is an axis-angle representation of mounting error. Turn this into a quaternion
qReset = AQ.quatFromAxisAngle(self.state[6:9]/(errorAngle + 1e-13),errorAngle)
# Push error onto reference quaternion
self.qReference = qReset*self.qReference
# We don't want to estimate a heading offset (seemed to be getting us into trouble)
eulers = self.qReference.asEuler
# set yaw to zero
eulers[2] = 0.
# reconstitute reference quaternion
self.qReference = AQ.Quaternion(eulers)
#self.qReference = AQ.Quaternion() # turn off mount error estimation
# zero Gibbs vector
self.state[6:9] = np.zeros((3,1))
def _resetQuat(self):
# In this step we push any mounting error calculated by the measurement update step
# onto the (non-stochastic) reference quaternion carried by the filter.
# Unpack Gibbs vector from state
errorAngle = np.linalg.norm(self.state[6:9])
# Gibbs vector is an axis-angle representation of mounting error. Turn this into a quaternion
qReset = AQ.quatFromAxisAngle(self.state[6:9] / (errorAngle + 1e-13),
errorAngle)
# Push error onto reference quaternion
self.qReference = qReset * self.qReference
# We don't want to estimate a heading offset (seemed to be getting us into trouble)
eulers = self.qReference.asEuler
# set yaw to zero
eulers[2] = 0.
# reconstitute reference quaternion
self.qReference = AQ.Quaternion(eulers)
#self.qReference = AQ.Quaternion() # turn off mount error estimation
# zero Gibbs vector
self.state[6:9] = np.zeros((3, 1))
def updateState(self,
dT,
motorCommands,
windVelocity=np.zeros((3, 1)),
disturbance=0,
externalForces=[]):
# Initialize force and moment vectors
Force = np.zeros((3, 1))
Moment = np.zeros((3, 1))
vel = np.array([self.stateVector[0, 3:6]]).T
omega = np.array([self.stateVector[0, 10:]]).T
self.v_Q_i = AQ.Quaternion(np.array(self.stateVector[0, 6:10]))
windVelBody = AQ.rotateVector(self.v_Q_i, windVelocity)
state = 'flying'
if (self.stateVector[0, 2] >= 0): # on ground?
if (np.dot(
AQ.rotateVector(self.v_Q_i.inv(), vel).T,
np.array([[0, 0, 1]]).T) > 2.): # falling fast
state = 'ground' #'crashed'
else:
state = 'ground'
#print state
if (state == 'crashed'):
return self.stateVector
# update all submodules (motors) and retrieve forces and moments
for ii in range(len(self.motorList)):
# update command
self.motorList[ii].commandMotor(motorCommands[ii])
# integrate state
self.motorList[ii].updateState(dT,
vehicleVelocity=vel,
vehicleOmega=omega,
windVelocity=windVelBody +
disturbance * np.random.randn(3, 1))
# get aero forces and moments from motor
FandM = self.motorList[ii].getForcesAndMoments()
# Sum all forces
Force = Force + FandM[0]
# Sum all moments + r_motor X F_motor
Moment = Moment + FandM[1] + np.cross(
self.motorList[ii].position.T, FandM[0].T).T
# add external forces, if any
# each entry in externalForces must be a tuple, the first entry of which is the applied force, and the second of which is the application location.
# both of these should be in body frame
for exFor in externalForces:
Force += exFor[0]
Moment += np.cross(exFor[1].T, exFor[0].T).T
# add drag force
relWindVector = vel - windVelBody
windMagnitude = np.sqrt(np.dot(relWindVector.T, relWindVector))
eWind = relWindVector / (windMagnitude + .00000000001)
dragForce = -0.5 * airDensity * self.cD * windMagnitude**2 * eWind
Force = Force + dragForce
# add gravity
Force = Force + self.mass * np.dot(self.v_Q_i.asRotMat, self.gravity)
#Force = Force + self.mass*self.gravity
# add ground force
if (self.stateVector[0, 2] >= 0):
# Add normal force
Kground = 1000
Kdamp = 50
Kangle = .001
groundForceMag = Kground * self.stateVector[0, 2] + Kdamp * np.dot(
self.v_Q_i.asRotMat, vel)[2, 0]
roll, pitch, yaw = self.v_Q_i.asEuler
groundPitchMoment = max(
min(
Kangle *
(-Kdamp * pitch - 6 * Kdamp * self.stateVector[0, 11]), 1),
-1)
groundRollMoment = max(
min(
Kangle *
(-Kdamp * roll - 6 * Kdamp * self.stateVector[0, 10]), 1),
-1)
groundYawMoment = Kangle * (-6 * Kdamp * self.stateVector[0, 12])
Force = Force + AQ.rotateVector(
self.v_Q_i,
np.array([[0, 0, -groundForceMag]]).T) - Kdamp * vel
Moment = Moment + np.array(
[[0., groundPitchMoment, 0.]]).T + np.array([[
groundRollMoment, 0., 0.
]]).T + np.array([[0., 0., groundYawMoment]]).T
self.externalForce = Force
self.externalMoment = Moment
#print state, self.mass*np.dot(self.v_Q_i.asRotMat,self.gravity).T
# solve eoms
#y = sc.integrate.odeint(self.vehicleEOM,self.stateVector[0,:],np.array([self.lastTime,self.lastTime + dT]))
attitude = AQ.Quaternion(np.array(self.stateVector[0, 6:10]))
magOmega = np.linalg.norm(omega)
axis = 1. / (magOmega + 1e-15) * omega
updateQuat = AQ.quatFromAxisAngle(axis, dT * magOmega)
self.stateVector = self.stateVector + dT * np.array(
[self.vehicleEOM(self.stateVector[0, :], 0)])
newAtt = updateQuat * attitude
self.stateVector[0, 6:10] = newAtt.q
#self.stateVector[0,:] = y[-1][:]
# Experiments with stupid Euler integration:
#y = self.stateVector[0,:] + dT*self.vehicleEOM(self.stateVector[0,:],0)
#self.stateVector[0,6:10] = AQ.normalize(self.stateVector[0,6:10])
self.lastTime = self.lastTime + dT
# update rotation matrix
#print 'euler angles: ',self.v_Q_i.asEuler, 'rates: ',self.stateVector[0,10:]
#print 'position: ', self.stateVector[0,0:3]
measuredAcceleration = 1. / self.mass * (
self.externalForce -
self.mass * np.dot(self.v_Q_i.asRotMat, self.gravity))
return [self.stateVector, measuredAcceleration]
def quatFromRotVec(vec):
angle = np.linalg.norm(vec)
if (angle == 0.):
return AQ.Quaternion([0.,0.,0.,1.])
axis = vec/angle
return AQ.quatFromAxisAngle(axis,angle)
def commandAngle(self, ang = 0,axis = np.array([[1,0,0]]).T ):
self.m_Q_v = AQ.quatFromAxisAngle(axis,ang)
def quatFromRotVec(vec):
angle = np.linalg.norm(vec)
if (angle == 0.):
return AQ.Quaternion([0.,0.,0.,1.])
axis = vec/angle
return AQ.quatFromAxisAngle(axis,angle)
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 updateState(self, dT, motorCommands, windVelocity=np.zeros((3, 1)), disturbance=0, externalForces=[]):
# Initialize force and moment vectors
Force = np.zeros((3, 1))
Moment = np.zeros((3, 1))
vel = np.array([self.stateVector[0, 3:6]]).T
omega = np.array([self.stateVector[0, 10:]]).T
self.v_Q_i = AQ.Quaternion(np.array(self.stateVector[0, 6:10]))
windVelBody = AQ.rotateVector(self.v_Q_i, windVelocity)
state = "flying"
if self.stateVector[0, 2] >= 0: # on ground?
if np.dot(AQ.rotateVector(self.v_Q_i.inv(), vel).T, np.array([[0, 0, 1]]).T) > 2.0: # falling fast
state = "ground" #'crashed'
else:
state = "ground"
# print state
if state == "crashed":
return self.stateVector
# update all submodules (motors) and retrieve forces and moments
for ii in range(len(self.motorList)):
# update command
self.motorList[ii].commandMotor(motorCommands[ii])
# integrate state
self.motorList[ii].updateState(
dT,
vehicleVelocity=vel,
vehicleOmega=omega,
windVelocity=windVelBody + disturbance * np.random.randn(3, 1),
)
# get aero forces and moments from motor
FandM = self.motorList[ii].getForcesAndMoments()
# Sum all forces
Force = Force + FandM[0]
# Sum all moments + r_motor X F_motor
Moment = Moment + FandM[1] + np.cross(self.motorList[ii].position.T, FandM[0].T).T
# add external forces, if any
# each entry in externalForces must be a tuple, the first entry of which is the applied force, and the second of which is the application location.
# both of these should be in body frame
for exFor in externalForces:
Force += exFor[0]
Moment += np.cross(exFor[1].T, exFor[0].T).T
# add drag force
relWindVector = vel - windVelBody
windMagnitude = np.sqrt(np.dot(relWindVector.T, relWindVector))
eWind = relWindVector / (windMagnitude + 0.00000000001)
dragForce = -0.5 * airDensity * self.cD * windMagnitude ** 2 * eWind
Force = Force + dragForce
# add gravity
Force = Force + self.mass * np.dot(self.v_Q_i.asRotMat, self.gravity)
# Force = Force + self.mass*self.gravity
# add ground force
if self.stateVector[0, 2] >= 0:
# Add normal force
Kground = 1000
Kdamp = 50
Kangle = 0.001
groundForceMag = Kground * self.stateVector[0, 2] + Kdamp * np.dot(self.v_Q_i.asRotMat, vel)[2, 0]
roll, pitch, yaw = self.v_Q_i.asEuler
groundPitchMoment = max(min(Kangle * (-Kdamp * pitch - 6 * Kdamp * self.stateVector[0, 11]), 1), -1)
groundRollMoment = max(min(Kangle * (-Kdamp * roll - 6 * Kdamp * self.stateVector[0, 10]), 1), -1)
groundYawMoment = Kangle * (-6 * Kdamp * self.stateVector[0, 12])
Force = Force + AQ.rotateVector(self.v_Q_i, np.array([[0, 0, -groundForceMag]]).T) - Kdamp * vel
Moment = (
Moment
+ np.array([[0.0, groundPitchMoment, 0.0]]).T
+ np.array([[groundRollMoment, 0.0, 0.0]]).T
+ np.array([[0.0, 0.0, groundYawMoment]]).T
)
self.externalForce = Force
self.externalMoment = Moment
# print state, self.mass*np.dot(self.v_Q_i.asRotMat,self.gravity).T
# solve eoms
# y = sc.integrate.odeint(self.vehicleEOM,self.stateVector[0,:],np.array([self.lastTime,self.lastTime + dT]))
attitude = AQ.Quaternion(np.array(self.stateVector[0, 6:10]))
magOmega = np.linalg.norm(omega)
axis = 1.0 / (magOmega + 1e-15) * omega
updateQuat = AQ.quatFromAxisAngle(axis, dT * magOmega)
self.stateVector = self.stateVector + dT * np.array([self.vehicleEOM(self.stateVector[0, :], 0)])
newAtt = updateQuat * attitude
self.stateVector[0, 6:10] = newAtt.q
# self.stateVector[0,:] = y[-1][:]
# Experiments with stupid Euler integration:
# y = self.stateVector[0,:] + dT*self.vehicleEOM(self.stateVector[0,:],0)
# self.stateVector[0,6:10] = AQ.normalize(self.stateVector[0,6:10])
self.lastTime = self.lastTime + dT
# update rotation matrix
# print 'euler angles: ',self.v_Q_i.asEuler, 'rates: ',self.stateVector[0,10:]
# print 'position: ', self.stateVector[0,0:3]
measuredAcceleration = (
1.0 / self.mass * (self.externalForce - self.mass * np.dot(self.v_Q_i.asRotMat, self.gravity))
)
return [self.stateVector, measuredAcceleration]
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]