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 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 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