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())
############################################################################
# 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!'
# create quadrotor
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()
PosFilt = KinematicEKF.PositionComplementaryFilter();
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]
#! /usr/bin/python
import numpy as np
import AeroQuaternion as AQ
import KinematicEKF as kEKF
filter = kEKF.kinematicQuadEKF()
accelerometer = np.array([[.01, .01, -9.82]]).T
gravMeas = np.array([[.0, .0, -9.81]]).T
gyro = np.array([[.01, .01, .01]]).T
velocity = np.zeros((3, 1))
pos = velocity
dT = 0.01
for ii in range(0, 100):
pos = pos + velocity * dT
velocity = velocity + (accelerometer - gravMeas) * dT
gps = ['gps', pos, np.eye(3)]
other = [gps]
stateAndCov = filter.runFilter(accelerometer, gyro, other, dT)
print 'state: '
print stateAndCov[0].T
#! /usr/bin/python import numpy as np import AeroQuaternion as AQ import KinematicEKF as kEKF filter = kEKF.kinematicQuadEKF() accelerometer = np.array([[.01,.01,-9.82]]).T gravMeas = np.array([[.0,.0,-9.81]]).T gyro = np.array([[.01,.01,.01]]).T velocity = np.zeros((3,1)) pos = velocity dT = 0.01 for ii in range(0,100): pos = pos + velocity*dT velocity = velocity + (accelerometer-gravMeas)*dT gps = ['gps',pos,np.eye(3)] other = [gps] stateAndCov = filter.runFilter(accelerometer,gyro,other,dT) print 'state: ' print stateAndCov[0].T
# and initialize sim stuff ################################### 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.]):
pose.rotational_velocity.x = state[0,10]
pose.rotational_velocity.y = state[0,11]
pose.rotational_velocity.z = state[0,12]
socket.send( pose.SerializeToString() )
def processControlData(data):
# Fill this in based on how controls are passed
commands = [data.roll, data.pitch, data.yaw_rate, data.throttle]
return commands
# create objects
Quad = Multirotor.Multirotor() # default is quadrotor
controller = QuadrotorController.Controller()
AttEstimator = KinematicEKF.AttitudeComplementaryFilter()
EKF = KinematicEKF.kinematicQuadEKF()
Quad.stateVector[0,2] = 0. # initial height
np.random.seed([])
gravTracker = 0.
accMeas = np.zeros((3,1))
lastUpdateTime = 0.
mountError = AQ.Quaternion(np.array([mount_error_roll,mount_error_pitch ,mount_error_yaw]))
# Set up ZMQ subscriber/poller stuff
poller = zmq.Poller()
poller.register(ControlReceiver, zmq.POLLIN)
Controldata = state_pb2.AngleCommandPb()
# create quadrotor
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()
PosFilt = KinematicEKF.PositionComplementaryFilter()
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]
