Quad = Multirotor.Multirotor() # default is quadrotor idx = 0 dt = 0.005 T = 1.3 numsteps = 3 maxInd = int(math.ceil(T/dt)) stateHist = np.zeros((numsteps*maxInd,13)) # command motor commands = [.5,.5,.5,.5] #commands = [0,0,0,0] # add wind windvel = np.zeros((3,1)) # run for a while controller = QuadrotorController.Controller() EKF = KinematicEKF.kinematicQuadEKF() AttEstimator = KinematicEKF.AttitudeComplementaryFilter() reference = [0.,0.,0.,3.] time = 0. period = dt ''' for jj in range(int(numsteps)): 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:
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()) ############################################################################ # here's the real script ############################################################################ EKF = KinematicEKF.kinematicQuadEKF() AttitudeFilt = KinematicEKF.AttitudeComplementaryFilter() lastTime = 0. # Set up ZMQ subscriber/poller stuff poller = zmq.Poller() poller.register(IMUReceiver, zmq.POLLIN) #poller.register(VICONReceiver, zmq.POLLIN) IMUdata = state_pb2.ImuPb() VICONdata = state_pb2.PosePb() firstPass = True firstPlot = True ii = 0 jj = 0 while (True): print 'loop!' # read in data from zmq
################################### Quad = Multirotor.Multirotor(fuselageMass = 0.5) # default is quadrotor idx = 0 dt = 0.002 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 #windvel = np.array([[30.,30.,0]]).T windvel = np.zeros((3,1)) # run for a whil controller = QuadrotorController.Controller() EKF = KinematicEKF.kinematicQuadEKF() AttEstimator = KinematicEKF.AttitudeComplementaryFilter(initialAttitude=AQ.Quaternion(np.array(attEst)), timeConstant = 10.,Kbias = .00) reference = [0.,0.,5.,0.] time = 0. lastTime = 0. startTime = clock.time() period = dt Quad.stateVector[0,2] = 0. # initial height Quad.stateVector[0,6:10] = AQ.Quaternion(np.array([0,0,0])).q #np.random.seed([]) accMeas = np.zeros((3,1)) def drawQuad(att = [0,0,0], pos = np.zeros((3,1)), color = [1.,0.,0.,1.]): global Quad