def vehicleEOM(self, y, t0):
# Equations of motion for vehicle
# Assumes that angular momentum from spinning rotors is negligible
# Assumes that forces and moments are constant during a timestep
# (should be reasonable for realistic timesteps, like 1ms)
#
# position derivatives equal rotated body velocities
xDot = AQ.rotateVector(self.v_Q_i.inv(), y[3:6])
# acceleration = F/m
vDot = 1 / self.mass * self.externalForce.T[0, :] - np.cross(np.array(y[10:]), np.array(y[3:6])).T
# update quaternnions
att = AQ.Quaternion(np.array(y[6:10]))
# qDot = att.inv()*AQ.vectorAsQuat(.5*np.array([y[10:]]).T)
qDot = AQ.qDotFromOmega(att, np.array(y[10:]))
# omegadots
effectiveMoment = (
self.externalMoment - np.array([np.cross(np.array(y[10:]), np.dot(self.Inertia, np.array(y[10:])))]).T
)
omegadots = np.dot(self.invInertia, effectiveMoment).T[0, :]
# print 'xdot: ',xDot.T[0]
# print 'vdot: ',vDot
# print 'qdot: ',qDot
# print 'wdot: ',omegadots
yDot = np.concatenate((xDot.T[0], vDot, qDot, omegadots), 1)
return yDot
def vehicleEOM(self, y, t0):
# Equations of motion for vehicle
# Assumes that angular momentum from spinning rotors is negligible
# Assumes that forces and moments are constant during a timestep
# (should be reasonable for realistic timesteps, like 1ms)
#
# position derivatives equal rotated body velocities
xDot = AQ.rotateVector(self.v_Q_i.inv(), y[3:6])
# acceleration = F/m
vDot = 1 / self.mass * self.externalForce.T[0, :] - np.cross(
np.array(y[10:]), np.array(y[3:6])).T
# update quaternnions
att = AQ.Quaternion(np.array(y[6:10]))
#qDot = att.inv()*AQ.vectorAsQuat(.5*np.array([y[10:]]).T)
qDot = AQ.qDotFromOmega(att, np.array(y[10:]))
# omegadots
effectiveMoment = self.externalMoment - np.array([
np.cross(np.array(y[10:]), np.dot(self.Inertia, np.array(y[10:])))
]).T
omegadots = np.dot(self.invInertia, effectiveMoment).T[0, :]
#print 'xdot: ',xDot.T[0]
#print 'vdot: ',vDot
#print 'qdot: ',qDot
#print 'wdot: ',omegadots
yDot = np.concatenate((xDot.T[0], vDot, qDot, omegadots))
return yDot
def publishEstimatedState(time, state):
x, y, z, u, v, w, qx, qy, qz, qw, p, q, r = state[0]
pose = state_pb2.PosePb()
pose.name = 'SimulatedPose'
pose.valid = True
pose.timestamp.seconds = time
# need real timestamp eventually
Q1 = Quat.Quaternion(np.array([180, 0, 0]))
pose.translation.x = x
pose.translation.y = y
pose.translation.z = z
Q = Quat.Quaternion(state[0, 6:10])
xform = Q #Q1*Q
#pose.rotation.x = state[0,6]
#pose.rotation.y = state[0,7]
#pose.rotation.z = state[0,8]
#pose.rotation.w = state[0,9]
pose.rotation.x = xform.q[0]
pose.rotation.y = xform.q[1]
pose.rotation.z = xform.q[2]
pose.rotation.w = xform.q[3]
#vel_inertial = Quat.rotateVector(Q.inv(),state[0,3:6]).T
pose.velocity.x = state[0, 3]
pose.velocity.y = state[0, 4]
pose.velocity.z = state[0, 5]
pose.rotational_velocity.x = state[0, 10]
pose.rotational_velocity.y = state[0, 11]
pose.rotational_velocity.z = state[0, 12]
socketEKF.send(pose.SerializeToString())
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 getForcesAndMoments(self):
# vehicleVelocity and vehicleOmega should be I_v_bcm and I_w_veh, in vehicle coords.
# windvelocity should have already been rotated into vehicle coords.
# returns forces and moments in vehicle reference frame
localWind = -(self.localVelAirRel)
omega = self.motorState[1]
FandM = self.prop.calculateForcesAndMoments(omega,localWind,self.direction)
output=[]
output.append(AQ.rotateVector(self.m_Q_v.inv(),FandM[0]))
output.append(AQ.rotateVector(self.m_Q_v.inv(),FandM[1]))
return FandM
def display():
startTime = clock.time()
global mouse_handler
global robot
position = robot.position()
attitude = robot.attitude_rad()
attitude_deg = 180 / np.pi * attitude
glLoadIdentity()
gl_R_ned = AQ.Quaternion(np.array([0, 180, -90]))
veh_R_ned = AQ.Quaternion(attitude_deg)
veh_R_yaw = AQ.Quaternion(np.array([0, 0, attitude_deg[2]]))
chaseCamPos = np.dot(
gl_R_ned.asRotMat,
np.dot(veh_R_yaw.asRotMat.T, np.array([[-5], [0], [-2]])))
posGL = np.dot(gl_R_ned.asRotMat, position)
if (mouse_handler.camera_mode == 'CHASE_CAM'):
gluLookAt(posGL[0] + chaseCamPos[0], posGL[1] + chaseCamPos[1],
posGL[2] + chaseCamPos[2], posGL[0], posGL[1], posGL[2], 0,
0, 1)
elif (mouse_handler.camera_mode == 'GROUND_CAM'):
gluLookAt(0, -10, 2, posGL[0], posGL[1], posGL[2], 0, 0, 1)
elif (mouse_handler.camera_mode == 'ONBOARD_CAM'):
veh_rider = np.dot(
gl_R_ned.asRotMat, position +
np.dot(veh_R_ned.asRotMat.T, np.array([[2], [0], [-.20]])))
veh_forward = posGL + np.dot(
gl_R_ned.asRotMat,
np.dot(veh_R_ned.asRotMat.T, np.array([[1000], [0], [0]])))
veh_up = np.dot(
gl_R_ned.asRotMat,
np.dot(veh_R_ned.asRotMat.T, np.array([[0], [0], [-1]])))
gluLookAt(veh_rider[0], veh_rider[1], veh_rider[2], veh_forward[0],
veh_forward[1], veh_forward[2], veh_up[0], veh_up[1],
veh_up[2])
else:
gluLookAt(0, -30, 10, 0, 0, 0, 0, 3, 1)
glPushMatrix()
# rotate into aero convention
glRotate(180., 1., 1., 0)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glColor4f(0., 0., 1., .8)
color = [0, 0., 1., 1.]
glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, color)
# draw ground
drawingUtils.drawEnvironment()
drawingUtils.drawAxes(.01, .1)
# draw robot
color = [1., 1., 0., 1.]
robot.draw()
glPopMatrix()
glutSwapBuffers()
return
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 processIMUData(IMUdata, useMag=False):
time = IMUdata.time_ms
otherMeas = []
if (IMUdata.HasField("gyro")):
gx = IMUdata.gyro.x
gy = IMUdata.gyro.y
gz = IMUdata.gyro.z
gyroOut = np.array([[gx], [gy], [gz]])
else:
return [False, 0, 0, 0]
if (IMUdata.HasField("accel")):
ax = IMUdata.accel.x
ay = IMUdata.accel.y
az = IMUdata.accel.z
#accOut = 9.81*np.array([[ax],[ay],[az]])#9.81*np.dot(accCalibMat,np.array([[ax],[ay],[az]]) - accoffset)
#accOut = 9.81*np.dot(accCalibMat,np.array([[ax],[ay],[az]]) - accoffset)
accOut = 9.81 * np.array([[ax], [ay], [az]])
else:
return [False, 0, 0, 0]
if (IMUdata.HasField("pose")):
qx = IMUdata.pose.rotation.x
qy = IMUdata.pose.rotation.y
qz = IMUdata.pose.rotation.z
qw = IMUdata.pose.rotation.w
attitude = AQ.Quaternion(np.array([qx, qy, qz, qw]))
if (IMUdata.HasField("mag") and useMag):
mx = IMUdata.mag.x
my = IMUdata.mag.y
mz = IMUdata.mag.z
magOut = np.array([[mx], [my], [mz]])
otherMeas.append(['mag', magOut, 1 * np.eye(3)])
return [True, time, gyroOut, accOut, attitude, otherMeas]
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 display():
#print startTime,
global position
global heading
global zoomLevel
global polygons
global path
glLoadIdentity()
# display parameters
gl_R_ned = AQ.Quaternion(np.array([0,180,-90]))
print position
posGL = np.dot(gl_R_ned.asRotMat,position)
gluLookAt(posGL[0],posGL[1], zoomLevel,
posGL[0],posGL[1],posGL[2],
0,1,0)
glPushMatrix()
# rotate into aero convention
glRotate(180.,1.,1.,0)
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
glColor4f(0.,0.,1.,.8)
color = [0,0.,.5,.1]
glMaterialfv(GL_FRONT_AND_BACK,GL_EMISSION,color)
# draw ground
drawEnvironment()
color = [1.,0.,0.,1.]
emissionColor = [.5,0.,0.,.1]
drawAxes()
glMaterialfv(GL_FRONT_AND_BACK,GL_DIFFUSE,color)
glMaterialfv(GL_FRONT_AND_BACK,GL_EMISSION,emissionColor)
# push quadrotor position on stack
glPushMatrix()
glTranslate(position[0,0],position[1,0],0)
glRotate(heading,0,0,1)
# draw quadrotor
drawQuad(color = [1.,0.,0.,1.])
# draw body axes
drawAxes()
glPopMatrix()
drawPolygons(polygons)
# change color
color = [0.0,1.,0.,1.]
glMaterialfv(GL_FRONT,GL_EMISSION,color)
drawPlannedPath(path)
glPopMatrix()
glutSwapBuffers()
#print clock.time() - startTime
return
def __init__(self,
initialState=np.array([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]],
dtype=np.float64).T,
initialCovariance=1 * np.eye(12, dtype=np.float64)):
self.state = initialState
self.cov = initialCovariance
self.gravEstimate = -gravity
self.qReference = AQ.Quaternion(
) # this is now a delta reference off the reported attitude
def __init__(self,initialState = np.array([[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]],dtype=np.float32).T,initialCovariance = 100000*np.eye(15,dtype=np.float32),processNoise = np.diag([.1,.1,.1, 1e-5, 1e-5, 1e-5, 1.,1.,1.,1e-5, 1e-5, 1e-5])):
self.state = initialState
self.cov = initialCovariance
self.cov[3:6,3:6] = .001*np.eye(3)
self.q = AQ.Quaternion([0,0,0])
self.processNoise = processNoise
self.initialized = False
self.initializationCounter = 100
self.firstGPS = False
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 publishState(time,state):
pose = state_pb2.PosePb()
pose.name = 'SimulatedPose'
pose.valid = True
pose.timestamp.seconds = time; # need real timestamp eventually
Q1 = Quat.Quaternion(np.array([180,0,0]))
pose.translation.x = state[0,0]
pose.translation.y = state[0,1]
pose.translation.z = state[0,2]
Q = Quat.Quaternion(state[0,6:10])
xform = Q #Q1*Q
#pose.rotation.x = state[0,6]
#pose.rotation.y = state[0,7]
#pose.rotation.z = state[0,8]
#pose.rotation.w = state[0,9]
pose.rotation.x = xform.q[0]
pose.rotation.y = xform.q[1]
pose.rotation.z = xform.q[2]
pose.rotation.w = xform.q[3]
socket.send( pose.SerializeToString() )
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 buildFmat(dT, state,accMeas,gyroMeas): x,y,z,vx,vy,vz,qx,qy,qz,qw,abx,aby,abz,bx,by,bz = state.T[0] att = AQ.Quaternion([qx,qy,qz,qw]) Fx = np.hstack( (dfxdx(), dfxdv(dT), 0.*dfxdq(dT,att,accMeas), dfxdaB(dT,att),np.zeros((3,3)) ) ) #Fx = np.hstack( (dfxdx(), dfxdv(dT), np.zeros((3,10) ) )) Fv = np.hstack( (np.zeros((3,3)) , dfvdv(), 0*dfvdq(dT,att,accMeas), dfvdaB(dT,att),np.zeros((3,3)) ) ) #Fv = np.hstack( (np.zeros((3,3)) , dfvdv(), np.zeros((3,10)) ) ) Fq = np.hstack( (np.zeros((4,6)), dfqdq(dT,gyroMeas), np.zeros((4,3)), dfqdwB(dT,att) ) ) Fb = np.zeros((6,16)) return np.vstack((Fx,Fv,Fq,Fb))
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 updateState(self,dT,vehicleVelocity = np.zeros((3,1)),vehicleOmega = np.zeros((3,1)),
windVelocity = np.zeros((3,1))):
# vehicle velocity, vehicle omega, and wind velocity are all given in vehicle frame
# first, calculate local velocity relative to the air
localVelAirRelVehFrame = vehicleVelocity + np.cross(vehicleOmega.T,self.position.T).T - windVelocity
# rotate into motor frame (in case we decide to tilt motors
self.localVelAirRel = AQ.rotateVector(self.m_Q_v,localVelAirRelVehFrame)
# solve ODEs
#y = sc.integrate.odeint(self.motorEOM,self.motorState,np.array([self.lastTime, self.lastTime + dT]))
#self.motorState = y[-1][:]
# faster?
#print 'motorState: ',self.motorState,'motorDeriv: ',self.motorEOM(self.motorState,dT)
self.motorState = self.motorState + dT*self.motorEOM(self.motorState,dT)
# then, calculate forces and moments
self.lastTime = self.lastTime + dT;
return self.motorState
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 __init__(self,
initialAttitude=AQ.Quaternion(),
timeConstant=15.,
Kbias=0.000):
'''
timeConstant is (roughly) the time it takes the filter to trust the accelerometer 100%
e.g. if you initialize the quad on an incline, the gyros will read zeros. With the default
timeConstant value of 10, it would take about ten seconds for the estimate of attitude to
converge to the true (tilted) attitude.
Kbias controls how quickly the filter learns a gyro bias. Too big and your quad's estimate will swing all over the place like
a drunkard. Too little and it won't learn any real bias that might exist. 0.01 seemed to work alright on real data.
'''
self.attitudeEstimate = initialAttitude
self.gyroBias = np.zeros((3, 1))
self.Kbias = Kbias
self.timeConstant = .1
self.tcTarget = timeConstant
def _predictStep(self,accMeas,gyroMeas,dT):
# unpack
#print self.state.T[0]
x,y,z,vx,vy,vz,qx,qy,qz,qw,abx,aby,abz,bx,by,bz = self.state.T[0]
# repack
att = AQ.Quaternion([qx,qy,qz,qw])
# Rotate accel into world
accBias = np.array([[abx,aby,abz]]).T
acc_world = np.dot(att.asRotMat.T,accMeas-accBias)
#print "acc_w: ",acc_world.T, qx, qy, qz, qw, abx,aby,abz
# subtract gravity
acc_net = acc_world - np.array([[0,0,-9.81]]).T
# pos update
xHat = np.array([[x,y,z]]).T + dT*np.array([[vx,vy,vz]]).T + 0.5*dT*dT*acc_net
# vel update
vHat = np.array([[vx,vy,vz]]).T + dT*acc_net
# quaternion state update
bias = np.array([[bx,by,bz]]).T
newAtt = quatFromRotVec((gyroMeas-bias)*dT)*att
newAtt.normalize()
# accel bias update
# constant
# gyro bias update
# constant
self.state = np.vstack((xHat,vHat,np.array([newAtt.q]).T,accBias,bias))
#
# Now do covariance
# Build "A matrix"
#
# TODO: Check transposes!!!
#
# Build F matrix
F = buildFmat(dT, self.state,acc_net,gyroMeas)
G = buildGmat(dT,att)
# noise
RT = .01*np.eye(6)
R = np.dot(G,np.dot(RT,G.T))
#R = 1e-6*np.eye(16)
# Inject some noise into the estimator
sigma_bias = .01
R[10:16,10:16] = sigma_bias*np.eye(6)
self.cov = np.dot(F,np.dot(self.cov,F.T)) + R
def processVICONData(VICONdata):
if VICONdata.valid:
x = VICONdata.translation.x
y = VICONdata.translation.y
z = VICONdata.translation.z
measPos = np.array([[x], [y], [z]
]) + .1 * np.array([np.random.randn(3)]).T
qx = VICONdata.rotation.x
qy = VICONdata.rotation.y
qz = VICONdata.rotation.z
qw = VICONdata.rotation.w
attitude = AQ.Quaternion(np.array([qx, qy, qz, qw]))
spoofMagMeas = np.dot(attitude.asRotMat, np.array([[1, 0, 0]]).T)
return [
True,
[['gps', measPos, 1 * np.eye(3)],
['mag', spoofMagMeas, .01 * np.eye(3)]]
]
else:
return [False, []]
def drawTether():
global Load
global Quad
global attitude
global tetherPoint
position = np.array([Quad.stateVector[0, 0:3]]).T
veh_R_ned = AQ.Quaternion(attitude)
tetherAnchor = position + np.dot(veh_R_ned.asRotMat.T, tetherPoint.copy())
dTether = tetherAnchor - Load.state[0:3]
stretch = np.sqrt(dTether[0, 0]**2 + dTether[1, 0]**2 + dTether[2, 0]**2)
glPushMatrix()
if (stretch >= Load.tetherLength):
glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, [0, 1, 0, 1])
else:
glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, [1, 0, 0, 1])
glBegin(GL_LINES)
glVertex3f(tetherAnchor[0, 0], tetherAnchor[1, 0], tetherAnchor[2, 0])
glVertex3f(Load.state[0, 0], Load.state[1, 0], Load.state[2, 0])
glEnd()
glPopMatrix()
#! /usr/bin/python import AeroQuaternion as AQ import numpy as np Q1 = AQ.Quaternion(np.array([0, 0, 0])) Q2 = AQ.Quaternion(np.array([0, 0, 90])) Q3 = AQ.Quaternion(np.array([0, 90, 0])) Q4 = AQ.Quaternion(np.array([0, 0, 180])) Q5 = AQ.Quaternion(np.array([90, 0, 0])) Q6 = AQ.Quaternion(np.array([0, 45, 0])) Q7 = AQ.Quaternion(np.array([45, 0, 0])) v = np.array([[0, 1, 0]]) print 'printing rotations of ', v.T print '0 : ', AQ.rotateVector(Q1, v).T print '90 yaw: ', AQ.rotateVector(Q2, v).T print 'with matrix: ', np.dot(Q2.asRotMat, v.T).T print '90 pitch: ', AQ.rotateVector(Q3, v).T print 'with matrix: ', np.dot(Q3.asRotMat, v.T).T print '90 roll: ', AQ.rotateVector(Q5, v).T, print 'with matrix: ', np.dot(Q5.asRotMat, v.T).T print '45 roll: ', AQ.rotateVector(Q7, v).T print 'with matrix: ', np.dot(Q7.asRotMat, v.T).T print '45 pitch: ', AQ.rotateVector(Q6, v).T print 'with matrix: ', np.dot(Q6.asRotMat, v.T).T print '180 yaw: ', AQ.rotateVector(Q4, v).T print 'with matrix: ', np.dot(Q4.asRotMat, v.T).T print 'Q7 as euler', Q7.asEuler
class Multirotor:
# Members
# Static parameters
mass = []
Inertia = []
invInertia = []
v_Q_i = AQ.Quaternion() # rotation matrix from intertial to body
cD = [] # drag coefficient
gravity = np.array([[0, 0, 9.81]]).T # gravity in inertial
motorList = []
externalForce = []
externalMoment = []
# State variables
# state = [x \
# y >-> Position of vehicle cm expressed in inertial frame
# z /
# u \
# v >-> velocity of vehicle cm in inertial, expressed in vehicle frame
# w /
# qx \
# qy >-> Attitude of vehicle in inertial, quaternion
# qz /
# qw /
# p \
# q >-> Angular velocity of body in inertial, expressed in body frame
# r /
stateVector = np.zeros((1, 13))
stateVector[0, 9] = 1. # unit quat
# code overhead
lastTime = -.01
# Methods
# constructor
def __init__(
self,
motorType="Motor",
propType="Propeller",
motorPositions=[
np.array([[.19, -.19, 0]]).T,
np.array([[.19, .19, 0]]).T,
np.array([[-.19, .19, 0]]).T,
np.array([[-.19, -.19, 0]]).T
],
motorOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1],
[0, 0, 0, 1]],
motorDirections=[1, -1, 1, -1],
fuselageMass=.7,
fuselageInertia=np.array([[.023385, 0, 0], [0, .018751, 0],
[0, 0, .030992]]),
dragCoefficient=.001,
):
print "Init called"
self.mass = fuselageMass
self.Inertia = fuselageInertia
self.cD = dragCoefficient
# Inertia accounting
Ixx = 0
Iyy = 0
Izz = 0
Ixy = 0
Ixz = 0
Iyz = 0
self.motorList = []
for ii in range(len(motorPositions)):
print "motorlist length: ", len(self.motorList)
print "motorpositions length: ", len(motorPositions)
# create motors (doing it with string allows for different prop/motor types
createString = (
"self.motorList.append(Motor." + motorType +
"(position=motorPositions[ii],direction = motorDirections[ii],m_Q_v=AQ.Quaternion(motorOrientations[ii]),prop=Propeller."
+ propType + "()))")
exec createString
motorMass = self.motorList[ii].mass
propMass = self.motorList[ii].prop.mass
# add mass of motor and props to total
self.mass = self.mass + motorMass + propMass
# motor/prop combos are treated as eccentric point masses for inertia calcs
Ixx = Ixx + (motorMass + propMass) * (motorPositions[ii][0]**2)[0]
Iyy = Iyy + (motorMass + propMass) * (motorPositions[ii][1]**2)[0]
Izz = Izz + (motorMass + propMass) * (motorPositions[ii][2]**2)[0]
Ixy = Ixy + (motorMass + propMass) * (motorPositions[ii][0] *
motorPositions[ii][1])[0]
Ixz = Ixz + (motorMass + propMass) * (motorPositions[ii][0] *
motorPositions[ii][2])[0]
Iyz = Iyz + (motorMass + propMass) * (motorPositions[ii][1] *
motorPositions[ii][2])[0]
# add effects of motor and prop intertias to the vehicle
Imotors = np.array([[Ixx, -Ixy, -Ixz], [-Ixy, Iyy, -Iyz],
[-Ixz, -Iyz, Izz]])
self.Inertia = self.Inertia + Imotors
self.invInertia = np.linalg.inv(self.Inertia)
print "inertia: ", self.Inertia, 'inverse inertia: ', self.invInertia
print len(self.motorList)
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 vehicleEOM(self, y, t0):
# Equations of motion for vehicle
# Assumes that angular momentum from spinning rotors is negligible
# Assumes that forces and moments are constant during a timestep
# (should be reasonable for realistic timesteps, like 1ms)
#
# position derivatives equal rotated body velocities
xDot = AQ.rotateVector(self.v_Q_i.inv(), y[3:6])
# acceleration = F/m
vDot = 1 / self.mass * self.externalForce.T[0, :] - np.cross(
np.array(y[10:]), np.array(y[3:6])).T
# update quaternnions
att = AQ.Quaternion(np.array(y[6:10]))
#qDot = att.inv()*AQ.vectorAsQuat(.5*np.array([y[10:]]).T)
qDot = AQ.qDotFromOmega(att, np.array(y[10:]))
# omegadots
effectiveMoment = self.externalMoment - np.array([
np.cross(np.array(y[10:]), np.dot(self.Inertia, np.array(y[10:])))
]).T
omegadots = np.dot(self.invInertia, effectiveMoment).T[0, :]
#print 'xdot: ',xDot.T[0]
#print 'vdot: ',vDot
#print 'qdot: ',qDot
#print 'wdot: ',omegadots
yDot = np.concatenate((xDot.T[0], vDot, qDot, omegadots))
return yDot
zCmd = 10.
cutMotors = True
gpsGood = False
###################################
# Create Quadrotor object
# and initialize sim stuff
###################################
MotorHeightOffset = 0.
motorPos = [np.array([[.19,-.19,MotorHeightOffset]]).T,
np.array([[.19,.19,MotorHeightOffset]]).T,
np.array([[-.19,.19,MotorHeightOffset]]).T,
np.array([[-.19,-.19,MotorHeightOffset]]).T]
print "initializing"
Quad = Multirotor.Multirotor(fuselageMass = .5,motorPositions = motorPos) # default is quadrotor
print "line 45: ",len(Quad.motorList)
initialAtt = AQ.Quaternion(attitude)
Quad.stateVector[0,6] = initialAtt.q[0]
Quad.stateVector[0,7] = initialAtt.q[1]
Quad.stateVector[0,8] = initialAtt.q[2]
Quad.stateVector[0,9] = initialAtt.q[3]
#while(True):
# pass
idx = 0
dt = 0.002
dtControl = .008
T = 1.3
numsteps = 3
maxInd = int(math.ceil(T/dt))
stateHist = np.zeros((numsteps*maxInd,13))
commands = [0.5,0.5,0.5,0.5]
# add wind
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 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 runDynamics():
global yaw
global height
global windvel
global period
global commands
global Quad
global EKF
global reference
global position
global attitude
global attEst
global startTime
global refType
global cutMotors
global yawCmd
global lastTime
global lastGPS
global gpsGood
global lastMAG
global MAG_FREQ
global dtControl
global lastControlTime
global gyroNoise
global accelNoise
# timing stuff
Time = clock.time()
dT = Time - runDynamics.lastTime
wind = windvel # + 10*np.random.randn(3,1)
if (dT < period):
return
# else update state
disturbance = 10
if (Time - startTime > 2 and Time - startTime < 10):
pass #w/ind = np.array([[10,0,0]]).T
state,acc = Quad.updateState(dt,commands,windVelocity = wind,disturbance = disturbance)
if ( Time - lastTime > 1. ):
print dT, 'x y ht: ', state[0,0], ' ', state[0,1], ' ', state[0,2]
lastTime = Time
# simulate measurements
accMeas = acc + accelNoise*np.array([np.random.randn(3)]).T + np.array([[0.0],[.0],[.0]])
gyroMeas = state.T[10:] + gyroNoise*np.array([np.random.randn(3)]).T + np.array([[.0],[.0],[.0]]) #+ np.array
# set it askew
accMeas = np.dot(AQ.Quaternion([0,0,0]).asRotMat,accMeas)
gyroMeas = np.dot(AQ.Quaternion([0,0,0]).asRotMat,gyroMeas)
attTrue = AQ.Quaternion(state[0,6:10])
#earthMagReading = np.array([[.48407,.12519,.8660254]]).T
earthMagReading = np.array([[.48407,.12519,.0]]).T
#earthMagReading = np.array([[2., 0., 10. ]]).T
earthMagReading = 1./np.linalg.norm(earthMagReading)*earthMagReading
magMeas = np.dot(attTrue.asRotMat,earthMagReading) + .01*np.array([np.random.randn(3)]).T
magCov = .1*np.eye(3)
otherMeas = []
# gps update?
if (Time - lastControlTime > dtControl and Time - lastMAG >= 1./MAG_FREQ):
otherMeas.append(['mag',magMeas,magCov,earthMagReading])
lastMAG = Time
if (Time - lastControlTime > dtControl and Time - lastGPS >= .2):
velWorld = np.dot(attTrue.asRotMat.T,state[0:1,3:6].T)
#gpsMeas = np.vstack((state[0:1,0:3].T,velWorld)) + .01*np.array([np.random.randn(3)]).T + np.array([[.0],[.0],[.0],[.0],[.0],[-.1]])
gpsMeas1 = state[0:1,0:3].T + .01*np.array([np.random.randn(3)]).T + np.array([[.0],[.0],[.0]]) + np.dot(attTrue.asRotMat.T,np.array([[1.,0.,0.]]).T)
gpsMeas2 = state[0:1,0:3].T + .01*np.array([np.random.randn(3)]).T + np.array([[.0],[.0],[.0]]) + np.dot(attTrue.asRotMat.T,np.array([[-1.,0.,0.]]).T)
#print gpsMeas
if (gpsGood):
otherMeas.append(['gps',gpsMeas1,np.diag([1,1,50]),np.array([[1.,0.,0.]]).T])
otherMeas.append(['gps',gpsMeas2,np.diag([1,1,50]),np.array([[-1.,0.,0.]]).T])
lastGPS = Time
# run attitude filter
otherMeas.append(['baro',state[0:1,2:3],np.array([[.1]]) ])
if (Time - lastControlTime > dtControl):
stateAndCovariance = EKF.runFilter(accMeas,gyroMeas,otherMeas,dtControl)
lastControlTime = Time
# update control
controlState = state.copy()
qx,qy,qz,qw = EKF.q.q
controlState[0,6] = qx
controlState[0,7] = qy
controlState[0,8] = qz
controlState[0,9] = qw
controlState[0,0] = EKF.state[0,0]
controlState[0,1] = EKF.state[1,0]
controlState[0,2] = EKF.state[2,0]
if (refType == 'xyah'):
reference[3] = yawCmd
if (refType == 'rpya'):
#dyrdt = state[0,4]
#yawCmd += dyrdt
reference[2] = yawCmd
if (cutMotors):
commands = controller.updateControl(dt,controlState,reference,'cut')
#commands = controller.updateControl(dt,state,reference,'cut')
else:
commands = controller.updateControl(dt,controlState,reference,refType)
#commands = controller.updateControl(dt,state,reference,refType)
if (False):
print 'commands: ',commands
print 'state: ',state
print 'dT: ',dT
#print Time - startTimes, 'altitude:', state[0,2]
attitude = attTrue.asEuler
attEst = EKF.q.asEuler
position[0,0] = state[0,0]
position[1,0] = state[0,1]
position[2,0] = state[0,2]
yaw = yaw + 1
height = 5*np.abs(np.sin(np.pi/180.*yaw))
# only update screen at reasonable rate
if (Time - runDynamics.lastFrameTime >= 1./20.):
# redisplay
glutPostRedisplay()
runDynamics.lastFrameTime = Time
runDynamics.lastTime = Time
def display():
startTime = clock.time()
global position
global attitude
global attEst
global cameraMode
global EKF
global Quad
positionEst = EKF.state[0:3,0:1]
glLoadIdentity()
gl_R_ned = AQ.Quaternion(np.array([0,180,-90]))
veh_R_ned = AQ.Quaternion(attitude)
veh_R_nedEst = AQ.Quaternion(attEst)
veh_R_yaw = AQ.Quaternion(np.array([0,0,attitude[2]]))
chaseCamPos = np.dot(gl_R_ned.asRotMat,np.dot(veh_R_yaw.asRotMat.T,np.array([[-30],[0],[-10]])))
posGL = np.dot(gl_R_ned.asRotMat,position)
posGLest = np.dot(gl_R_ned.asRotMat,positionEst)
if (cameraMode == 'CHASE_CAM'):
gluLookAt(posGL[0]+chaseCamPos[0],posGL[1]+chaseCamPos[1],posGL[2]+chaseCamPos[2],
posGL[0],posGL[1],posGL[2],
0,0,1)
elif (cameraMode == 'CHASE_CAM_EST'):
gluLookAt(posGLest[0]+chaseCamPos[0],posGLest[1]+chaseCamPos[1],posGLest[2]+chaseCamPos[2],
posGLest[0],posGLest[1],posGLest[2],
0,0,1)
elif (cameraMode == 'GROUND_CAM'):
gluLookAt(0,-30,2,
posGL[0],posGL[1],posGL[2],
0,0,1)
elif (cameraMode == 'ONBOARD_CAM'):
veh_rider = np.dot(gl_R_ned.asRotMat,position + np.dot(veh_R_ned.asRotMat.T,np.array([[2],[0],[-.20]])))
veh_forward = posGL + np.dot(gl_R_ned.asRotMat,np.dot(veh_R_ned.asRotMat.T,np.array([[1000],[0],[0]])))
veh_up = np.dot(gl_R_ned.asRotMat, np.dot(veh_R_ned.asRotMat.T,np.array([[0],[0],[-1]])))
gluLookAt(veh_rider[0],veh_rider[1],veh_rider[2],
veh_forward[0],veh_forward[1],veh_forward[2],
veh_up[0],veh_up[1],veh_up[2])
else:
gluLookAt(0,-30,10,
0,0,0,
0,3,1)
glPushMatrix()
# rotate into aero convention
glRotate(180.,1.,1.,0)
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
glColor4f(0.,0.,1.,.8)
color = [0,0.,1.,1.]
glMaterialfv(GL_FRONT_AND_BACK,GL_EMISSION,color)
# draw ground
drawingUtils.drawEnvironment()
drawingUtils.drawAxes()
# draw quadrotor
drawingUtils.drawQuad(att = attitude,color = [1.,0.,0.,1.],pos = position[0:3],quad = Quad)
# push quadrotor estimated position on stack
color = [1.,1.,0.,1.]
glMaterialfv(GL_FRONT,GL_EMISSION,color)
# Estimated quad
attEKF = EKF.q
attEulerEst = attEKF.asEuler
# draw quadrotor
drawingUtils.drawQuad(att=attEulerEst,color = [0.,1.,0.,1.],pos=positionEst[0:3],quad=Quad)
glPopMatrix()
glutSwapBuffers()
return
Q1 = AQ.Quaternion(np.array([0,0,0])) Q2 = AQ.Quaternion(np.array([0,0,90])) Q3 = AQ.Quaternion(np.array([0,90,0])) Q4 = AQ.Quaternion(np.array([0,0,180])) Q5 = AQ.Quaternion(np.array([90,0,0])) Q6 = AQ.Quaternion(np.array([0,45,0])) Q7 = AQ.Quaternion(np.array([45,0,0])) v = np.array([[0,1,0]]) print 'printing rotations of ',v.T print '0 : ', AQ.rotateVector(Q1,v).T print '90 yaw: ', AQ.rotateVector(Q2,v).T print 'with matrix: ', np.dot(Q2.asRotMat,v.T).T print '90 pitch: ', AQ.rotateVector(Q3,v).T print 'with matrix: ', np.dot(Q3.asRotMat,v.T).T print '90 roll: ', AQ.rotateVector(Q5,v).T, print 'with matrix: ', np.dot(Q5.asRotMat,v.T).T print '45 roll: ', AQ.rotateVector(Q7,v).T print 'with matrix: ', np.dot(Q7.asRotMat,v.T).T print '45 pitch: ', AQ.rotateVector(Q6,v).T print 'with matrix: ', np.dot(Q6.asRotMat,v.T).T print '180 yaw: ', AQ.rotateVector(Q4,v).T print 'with matrix: ', np.dot(Q4.asRotMat,v.T).T print 'Q7 as euler',Q7.asEuler
for ii in range(0,maxInd):
state = Quad.updateState(dt,commands,windVelocity=windvel)
#stateHist[idx,:] = state
idx = idx+1
publishState(idx*dt,state)
if(jj <= 1):
pass
commands = [.3,.3, .3, .4]
#commands = [0, 0, 0, 0]
else:
#commands = [.5,.5,.5,.5]
commands = [0,0,0,0]
print stateHist
'''
Quad.stateVector[0, 2] = -15. # initial height
Quad.stateVector[0, 6:10] = AQ.Quaternion(np.array([0, 30, 20])).q
np.random.seed([])
gravTracker = 0.
accMeas = np.zeros((3, 1))
lastUpdateTime = 0.
mountError = AQ.Quaternion(np.array([3., 1., 0]))
state = np.zeros((1, 13))
state[0, 6:10] = AQ.Quaternion(np.array([0, 10, 20])).q
while (True):
startTime = clock.time()
windvel = windvel + np.random.randn(3, 1)
disturbance = 10
if (time < 0):
acc = np.array([[0, 0, -9.81]]).T
#state = np.zeros((1,13))
state[0, 6:10] = AQ.Quaternion(np.array([0, 0, 20])).q
publishState(idx*dt,state)
if(jj <= 1):
pass
commands = [.3,.3, .3, .4]
#commands = [0, 0, 0, 0]
else:
#commands = [.5,.5,.5,.5]
commands = [0,0,0,0]
print stateHist
'''
Quad.stateVector[0,2] = 0. # initial height
np.random.seed([])
gravTracker = 0.
accMeas = np.zeros((3,1))
lastUpdateTime = 0.
mountError = AQ.Quaternion(np.array([3.,1.,0]))
while(True):
startTime = clock.time()
windvel = windvel + np.random.randn(3,1)
disturbance = 10
if (time<20):
acc = np.array([[0,0,-9.81]]).T
state = np.zeros((1,13))
state[0,6:10] = AQ.Quaternion(np.array([0,0,20])).q
else:
state,acc = Quad.updateState(dt,commands,windVelocity = windvel,disturbance = disturbance)
#print 'norm acc: ', np.linalg.norm(acc)
accMeas = acc + 1*np.array([np.random.randn(3)]).T + np.array([[.0],[0],[0]])
# run EKF
gyroMeas = state.T[10:] + .01*np.array([np.random.randn(3)]).T + np.array([[0.001],[0],[0]]) #+ np.array([[0.],[0.01],[.0]])# + .001*np.ones((3,1))
#posMeas = state.T[0:3] + .1*(np.array([np.random.rand(3)]).T - .5*np.ones((3,1)))
def commandAngle(self, ang = 0,axis = np.array([[1,0,0]]).T ):
self.m_Q_v = AQ.quatFromAxisAngle(axis,ang)
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]
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 updateControl(self,dT,state,reference,refType):
x,y,z,u,v,w,qx,qy,qz,qw,p,q,r = state[0]
# extract eulers for control
attitude = AQ.Quaternion(np.array([qx,qy,qz,qw]))
worldVel = np.dot(attitude.asRotMat.T,np.array([[u],[v],[w]]))
roll,pitch,yaw = np.pi/180*attitude.asEuler # convert to rad
MAX_ANGLE = 30.*np.pi/180
MAX_VEL = 9.
MAX_VEL_Z = 3.
MAX_YR = 1.
if (refType == 'rpya'):
rollRef,pitchRef,yawRef,hRef = reference
rollRef = max(min(rollRef,MAX_ANGLE),-MAX_ANGLE)
pitchRef = max(min(pitchRef,MAX_ANGLE),-MAX_ANGLE)
yawRef = angleFromTo(yawRef,-np.pi,np.pi)
zErr = -hRef - z
self.heightErrorInt = self.heightErrorInt + zErr*dT
vzCmd = min(max(self.gains.alt.Ki*self.heightErrorInt + self.gains.alt.Kp*zErr,-MAX_VEL_Z),MAX_VEL_Z)
vzErr = (vzCmd - worldVel[2,0])
vzErrDeriv = (vzErr - self.lastvzErr)/dT
self.lastvzErr = vzErr
collective = min(max( .43 - self.gains.alt.Kp*vzErr - self.gains.alt.Kd*vzErrDeriv, 0),1)
self.posXerrorInt = 0.
self.posYerrorInt = 0.
self.heightErrorInt = 0.
self.yawErrorInt = 0.
elif (refType == 'xyah' or refType == 'cut'):
# Unpack references
xRef,yRef,zRef,yawRef = reference
yawRef = angleFromTo(yawRef,-np.pi,np.pi)
# compute errors
xErr = xRef - x
yErr = yRef - y
hRef = zRef
zErr = -hRef - z
yawErr = yawRef - yaw
#print 'errors (x, y, z, y):', xErr,yErr,zErr,yawErr
# calculate error integrals in world frame
self.posXerrorInt = self.posXerrorInt + xErr*dT
self.posYerrorInt = self.posYerrorInt + yErr*dT
self.heightErrorInt = self.heightErrorInt + zErr*dT
self.yawErrorInt = self.yawErrorInt + yawErr*dT
# rotate errors and integrals into body frame
xErrBody = xErr*np.cos(yaw) + yErr*np.sin(yaw)
yErrBody = -xErr*np.sin(yaw) + yErr*np.cos(yaw)
xErrIntBody = self.posXerrorInt*np.cos(yaw) + self.posYerrorInt*np.sin(yaw)
yErrIntBody = -self.posXerrorInt*np.sin(yaw) + self.posYerrorInt*np.cos(yaw)
# control gains
KiPos = 0.#self.gains.x.Ki
KpPos = self.gains.x.Kp
KpVel = self.gains.vx.Kp
MAX_VEL = 9.
MAX_YR = 1.
# Velocity gains from reference positions
vxCmd = KiPos*xErrIntBody + KpPos*xErrBody
vyCmd = KiPos*yErrIntBody + KpPos*yErrBody
vMag = np.sqrt(vxCmd**2 + vyCmd**2)
saturateFlag = False
if (vMag > MAX_VEL):
vxCmd = MAX_VEL*vxCmd/vMag
vyCmd = MAX_VEL*vyCmd/vMag
saturateFlag = True
#command vz and yaw rate
vzCmd = min(max(self.gains.alt.Ki*self.heightErrorInt + self.gains.alt.Kp*zErr,-MAX_VEL),MAX_VEL)
yrCmd = min(max(self.gains.yaw.Ki*self.yawErrorInt + self.gains.yaw.Kp*yawErr,-MAX_YR),MAX_YR)
#print 'commands (vx, vy, vz, yr):', vxCmd,vyCmd,vzCmd,yrCmd
# anti-windup
'''if(KiPos > 0):
if(vyCmd == MAX_VEL and yErrBody > 0.):
yErrIntBody = 1./KiPos*(MAX_VEL - KpPos*yErrBody)
saturateFlag = True
elif(vyCmd == -MAX_ANGLE and yErrBody < 0.):
yErrIntBody = 1./KiPos*(-MAX_VEL - KpPos*yErrBody)
saturateFlag = True
if(vxCmd == MAX_VEL and xErrBody > 0.):
xErrIntBody = 1./KiPos*(MAX_VEL - KpPos*xErrBody)
saturateFlag = True
elif(vxCmd == -MAX_VEL and xErrBody < 0.):
xErrIntBody = 1./KiPos*(-MAX_VEL - KpPos*xErrBody)
saturateFlag = True'''
if(saturateFlag):
#self.posXerrorInt = np.cos(yaw)*xErrIntBody - np.sin(yaw)*yErrIntBody
#self.posYerrorInt = np.sin(yaw)*xErrIntBody + np.cos(yaw)*yErrIntBody
self.posXerrorInt -= xErr*dT
self.posYerrorInt -= yErr*dT
# handle z
if(self.gains.alt.Ki > 0):
if(vzCmd == MAX_VEL and zErr > 0.):
self.heightErrorInt -= zErr*dT
elif(vzCmd == -MAX_VEL and zErr < 0.):
self.heightErrorInt -= zErr*dT
# handle yaw
if(self.gains.yaw.Ki > 0):
if(yrCmd == MAX_YR and yawErr > 0.):
self.yawErrorInt -= yawErr*dT
elif(yrCmd == -MAX_YR and yawErr < 0.):
self.yawErrorInt -= yawErr*dT
# form pitch and roll commands from velocity commands
vxErr = (vxCmd - (worldVel[0,0]*np.cos(yaw) + worldVel[1,0]*np.sin(yaw)))
vyErr = (vyCmd - (-worldVel[0,0]*np.sin(yaw) + worldVel[1,0]*np.cos(yaw)))
vzErr = (vzCmd - worldVel[2,0])
#print vxErr, vyErr,vzErr
# derivative terms
vxErrDeriv = (vxErr - self.lastvxErr)/dT
vyErrDeriv = (vyErr - self.lastvyErr)/dT
vzErrDeriv = (vzErr - self.lastvzErr)/dT
self.lastvxErr = vxErr
self.lastvyErr = vyErr
self.lastvzErr = vzErr
rollRef = min(max(self.gains.vy.Kp*vyErr + self.gains.vy.Kd*vyErrDeriv,-MAX_ANGLE),MAX_ANGLE)
pitchRef = min(max(-self.gains.vx.Kp*vxErr - self.gains.vx.Kd*vxErrDeriv,-MAX_ANGLE),MAX_ANGLE)
collective = min(max( .43 - self.gains.alt.Kp*vzErr - self.gains.alt.Kd*vzErrDeriv, 0),1)
#collective = min(max( .55 - self.gains.alt.Kp*vzErr - self.gains.alt.Kd*vzErrDeriv, 0),1)
elif (refType == 'rpYRt'):
rollRef,pitchRef,yawRateRef,collective = reference
#print 'reference',reference
yawRef = yaw
hRef = -z
# end anti-windup
#print 'attitude ', attitude.asEuler
# Prefilter commanded angles
# Ensure Yaw between -PI and PI
yawRef = angleFromTo(yawRef, -math.pi, math.pi)
# update filtered states
self.filteredRollCmd, self.filteredWxCmd = self.RollCmdFilter.filterAngle(rollRef,dT)
self.filteredPitchCmd, self.filteredWyCmd = self.PitchCmdFilter.filterAngle(pitchRef,dT)
self.filteredYawCmd, self.filteredWzCmd = self.YawCmdFilter.filterAngle(yawRef,dT)
self.filteredHtCmd, self.filteredZdotCmd = self.HtCmdFilter.filter(hRef,dT)
#collective,collDot = self.CollectiveFilter.filter(collective,dT)
#print "coll: ", collective
################################
## BEGIN LOW-LEVEL CONTROLLER
################################
# calculate errors
rollError = self.filteredRollCmd - roll
pitchError = self.filteredPitchCmd - pitch
yawError = angleFromTo(self.filteredYawCmd - yaw, -math.pi, math.pi)
#print rollError
#self.rollErrorInt += rollError*dT
#self.pitchErrorInt += pitchError*dT
rollRateError = self.filteredWxCmd - p
pitchRateError = self.filteredWyCmd - q
if (refType == 'rpYRt'):
yawRateError = angleFromTo(yawRateRef - r, -math.pi, math.pi)
else:
yawRateError = - r
heightError = self.filteredHtCmd -(-z)
heightRateError = self.filteredZdotCmd - (-w)
#print 'err ht: ',heightError,'ref: ',reference,'filt ht cmd: ',self.filteredHtCmd,'href: ',hRef
alf = .6
self.filtpDotErr = alf*self.filtpDotErr + (1-alf)*(rollRateError-self.lastWxError)/dT
self.filtqDotErr = alf*self.filtqDotErr + (1-alf)*(pitchRateError-self.lastWyError)/dT
self.filtrDotErr = alf*self.filtrDotErr + (1-alf)*(yawRateError-self.lastWzError)/dT
self.filtZddotErr = alf*self.filtZddotErr + (1-alf)*(heightRateError-self.lastZdotError)/dT
# update last rate errors
self.lastWxError = rollRateError
self.lastWyError = pitchRateError
self.lastWzError = yawRateError
self.lastZdotError = heightRateError
# done with errors
trimThrottle = .43 # Butt number
#print 'err, rpyh:', rollError,pitchError,yawError,heightError
#print 'gains rpy: ',self.gains.roll.Kp,self.gains.pitch.Kp,self.gains.yaw.Kp
# Error integrals
''' Motor numbering convention:
_ _
/1\ /2\
\_/ ^ \_/
\ Front /
\ /
\____/
| | Top view
|____|
/ \
/ \
_/ \_
/4\ /3\
\_/ \_/
'''
rollRateCmd = self.gains.roll.Kp*(self.filteredRollCmd - roll)
pitchRateCmd = self.gains.pitch.Kp*(self.filteredPitchCmd - pitch)
rollRateError = rollRateCmd - p
pitchRateError = pitchRateCmd - q
self.rollErrorInt += rollRateError*dT
self.pitchErrorInt += pitchRateError*dT
RollControl = self.gains.rollRate.Kp*rollRateError + self.gains.rollRate.Ki*self.rollErrorInt + self.gains.rollRate.Kd*self.filtpDotErr
PitchControl = self.gains.pitchRate.Kp*pitchRateError + self.gains.pitchRate.Ki*self.pitchErrorInt + self.gains.pitchRate.Kd*self.filtqDotErr
'''
RollControl = self.gains.roll.Kp*rollError + self.gains.roll.Ki*self.rollErrorInt + self.gains.rollRate.Kp*rollRateError + self.gains.rollRate.Kd*self.filtpDotErr
PitchControl = self.gains.pitch.Kp*pitchError + self.gains.pitch.Ki*self.pitchErrorInt + self.gains.pitchRate.Kp*pitchRateError + self.gains.pitchRate.Kd*self.filtqDotErr'''
YawControl = self.gains.yaw.Kp*yawError + self.gains.yaw.Ki*self.yawErrorInt + self.gains.yawRate.Kp*yawRateError + self.gains.yawRate.Ki*self.yawRateErrorInt \
+ self.gains.yawRate.Kd*self.filtqDotErr
if (refType == 'rpYRt' or refType == 'xyah' or refType == 'rpya'): # Control mixing for roll pitch yawRate throttle commands
out1 = collective \
+ RollControl \
+ PitchControl\
- YawControl
out2 = collective \
- RollControl \
+ PitchControl\
+ YawControl
out3 = collective \
- RollControl \
- PitchControl\
- YawControl
out4 = collective \
+ RollControl \
- PitchControl\
+ YawControl
elif (refType == 'cut'):
out1 = 0
out2 = 0
out3 = 0
out4 = 0
self.RollCmdFilter.reset()
self.PitchCmdFilter.reset()
self.YawCmdFilter.reset()
self.HtCmdFilter.reset()
else: # Control mixing for other modes
out1 = trimThrottle + self.gains.alt.Kp*heightError + self.gains.alt.Ki*self.heightErrorInt + self.gains.alt.Kd*heightRateError \
+ RollControl \
+ PitchControl\
- YawControl
out2 = trimThrottle + self.gains.alt.Kp*heightError + self.gains.alt.Ki*self.heightErrorInt + self.gains.alt.Kd*heightRateError \
- RollControl \
+ PitchControl\
+ YawControl
out3 = trimThrottle + self.gains.alt.Kp*heightError + self.gains.alt.Ki*self.heightErrorInt + self.gains.alt.Kd*heightRateError \
- RollControl \
- PitchControl\
- YawControl
out4 = trimThrottle + self.gains.alt.Kp*heightError + self.gains.alt.Ki*self.heightErrorInt + self.gains.alt.Kd*heightRateError \
+ RollControl \
- PitchControl\
+ YawControl
outs = [out1, out2, out3, out4]
maxOut = 0.
minOut = 1.
MAX_THRO = .95
MIN_THRO = 0.
for out in outs:
if (out > maxOut):
maxOut = out
elif (out < minOut):
minOut = out;
if (minOut < MIN_THRO or maxOut > MAX_THRO):
# undo integration then saturate
self.rollErrorInt -= rollRateError*dT
self.pitchErrorInt -= pitchRateError*dT
if (minOut < MIN_THRO and maxOut > MAX_THRO):
print "BOTH SAT"
for out in outs:
out = (out - minOut)*(MAX_THRO-MIN_THRO)/(maxOut - minOut) + MIN_THRO
elif (minOut < MIN_THRO): # low cap
print "LOW SAT"
for out in outs:
out = out + (MIN_THRO - minOut)
else: #hi cap
print "HIGH SAT"
for out in outs:
out = out - (maxOut - MAX_THRO)
#print ' output: ',np.array([out1,out2,out3,out4])
#print 'filteredYawCommand: ', self.filteredYawCmd
return np.array(outs)
def main(args):
if (len(sys.argv) < 2) :
# default
print 'usage: python cleanData.py [inputFileName] [outputFilePath]'
print 'using defaults:'
filename = 'extractedData.csv'
else:
# something exciting?
filename = sys.argv[1]
print 'input: ',filename
if (len(sys.argv) < 3):
outfilePath = ''
else:
outfilePath = sys.argv[2]
print 'output path: ', outfilePath
print 'subsample factor: ', _SUBSAMPLE_
csvfile = open(filename,'rb')
datareader = csv.reader(csvfile,delimiter=',')
header = next(datareader,None)
print header
time.sleep(1)
lastTime = 0.
scan = []
# read in data, group by timestamp
startPos = next(datareader,None)
print startPos
startTime = float(startPos[_TIME_])
lastTime = startTime-.333
startState,spd = stateParse(startPos)
lastState = np.zeros((6,1))
state = startState
logCount = 0
maxLogs = 70000
statehist = np.zeros((maxLogs,6))
#statehist[logCount] = state.T
dX = np.zeros((3,1))
vels = np.zeros((maxLogs,3))
omegas = np.zeros((maxLogs,1))
times = np.zeros((maxLogs,1))
beamCounters = np.zeros((maxLogs,1))
logCount = 0
# read in data, group by timestamp
beams_t = np.zeros((512,3))
beamCounter = 0
beamHist = []
startFlag = True
for row in datareader:
if logCount < maxLogs:
# record time
time_t = float(row[_TIME_])
if startFlag:
# initialize
logCount = 1
lastTime = time_t
times[0] = time_t
startFlag = False
else:
# run normally
if (time_t != lastTime):
# New timestamp.
# store beams from last timestamp
beamCounters[logCount-1] = beamCounter
beams_t_trim = beams_t[0:beamCounter,:]
beamHist.append(beams_t_trim)
# new state
dT = time_t - lastTime
times[logCount] = time_t
[state,speed] = stateParse(row,startState)
statehist[logCount] = state.T
#print statehist[logCount], state
omega = (state[5] - lastState[5])/(dT) # makes assumption that all w is in z direction
omegas[logCount] = omega
# Calculate DVL velocity
att = AQ.Quaternion([0.,0.,(180./np.pi)*(state[5])])
# finite differencing of pos for velocity requires a bit of smoothing
smoothPos = .7*(1.-.9**logCount) # time-varying gain allows quick convergence. Approaches 0.7 asymptotically quite quickly
dX = smoothPos*dX + (1.-smoothPos)*(state[0:3] - lastState[0:3])
# rotate dx into body frame for velocity
vel = np.dot(att.asRotMat,(1./(dT))*(dX))
#vel = 1./dT*dX
vels[logCount] = vel.T[0]
#print vels[logCount], vel.T
#print vel.T, att.asEule
logCount += 1
beams_t = np.zeros((512,3))
beam_t = beamParse(row);
beams_t[0,:] = beam_t[:,0]
beamCounter = 1
lastTime = time_t
lastState = state
if (logCount%1000 == 0):
print logCount
else:
# if we're not on a new timestamp
# process scan
if (beamCounter < 512):
beam_t = beamParse(row);
beams_t[beamCounter,:] = beam_t[:,0]
beamCounter += 1
beamHist.append(beams_t[0:beamCounter,:])
#print logCount
# unwrap psi and start at zero
initialHeading = statehist[0,5]
stateHist = statehist[0:logCount]
stateHist[:,5] = np.unwrap(stateHist[:,5])-initialHeading
Vels = vels[0:logCount]
Vels[0] = Vels[1]
Vels[:,0] = signal.medfilt(Vels[:,0],5);
Vels[:,1] = signal.medfilt(Vels[:,1],7);
Omegas = omegas[0:logCount]
Times = times[0:logCount] - times[0]
BeamCounters = beamCounters[1:logCount+1]
BeamCounters[-1] = beamCounter
# rotate positions to make consistent with new psi
psi0 = AQ.Quaternion([0.,0.,180./np.pi*initialHeading])
posRot = np.dot(psi0.asRotMat,stateHist.T[0:3,:])
rotatedPos = posRot.T
rotatedStateHist = np.hstack((rotatedPos,stateHist[:][:,3:]))
#for ii in range(Vels.shape[0]):
# print Vels[ii]
#plt.plot(Times)
#plt.plot(rotatedStateHist[:,0],rotatedStateHist[:,1],'r')
#plt.axis('equal')
#plt.legend(['vx','vy','vz'])
#plt.show()
# concatenate full state
bigMama = np.hstack((Times,rotatedStateHist,
np.ones((rotatedStateHist.shape[0],1)),
Omegas,
np.ones((rotatedStateHist.shape[0],1)),
Vels))
#print to file
np.savetxt(outfilePath+"fullstatehist.csv",bigMama,fmt='%1.6e',delimiter=",")
# save state and range readings
printScansToCSV(bigMama,beamHist,outfile=outfilePath+"state_and_ranges.csv")
def attFromAccel(yawInDeg, acc):
roll = math.atan2(-acc[1, 0], -acc[2, 0])
pitch = math.asin(acc[0, 0] / (np.linalg.norm(acc) + 1E-12))
return AQ.Quaternion(
np.array([180. / np.pi * roll, 180. / np.pi * pitch,
yawInDeg])) # constructor takes eulers in degrees
