def __init__(self,
mass,
inertiaMatrix,
omegaSqrToDragTorque,
disturbanceTorqueStdDev,
pos=Vec3(0, 0, 0),
vel=Vec3(0, 0, 0),
att=Rotation.identity(),
d=Vec3(0, 0, 0)):
self._mass = mass
self._inertia = inertiaMatrix
self._omegaSqrToDragTorque = omegaSqrToDragTorque
self._disturbanceTorqueStdDev = disturbanceTorqueStdDev
self._state_m = State(pos, vel, d, att)
self._state_p = State(pos, vel, d, att)
self._Pp = None
self._Pm = np.eye(9) * 0.01
self._stateHistP = []
self._posNoise = [0.01, 0.01, 0.01]
self._velNoise = [0.01, 0.01, 0.01]
self._dNoise = [0.01, 0.01, 0.01]
self._debug = False
def __init__(self, position, rotAxis, minSpeed, maxSpeed, speedSqrToThrust,
speedSqrToTorque, timeConst, inertia):
self._rotAxis = rotAxis
self._thrustAxis = Vec3(rotAxis)
if self._thrustAxis.z < 0:
self._thrustAxis = - self._thrustAxis
self._minSpeed = minSpeed
self._maxSpeed = maxSpeed
self._speedSqrToThrust = np.abs(speedSqrToThrust)
self._speedSqrToTorque = np.abs(speedSqrToTorque)
self._timeConst = timeConst
self._inertia = inertia
self._speed = None #self._minSpeed
self._position = position
self._thrust = Vec3(0,0,0)
self._torque = Vec3(0,0,0)
self._angularMomentum = Vec3(0,0,0)
self._powerConsumptionInstantaneous = 0
def linearizeDynamics(self, accImu, gyroImu, dt):
#create linearized A matrix
A = np.zeros((9, 9))
A_pos = np.eye(3)
A_d = np.eye(3)
#position to velocity model
A_posvel = self._state_m.att.to_rotation_matrix() * dt
#print(self._state_m.att.to_rotation_matrix)
#print(A_posvel)
#position to attitude model
d0 = Vec3(1, 0, 0).to_cross_product_matrix()
d1 = Vec3(0, 1, 0).to_cross_product_matrix()
d2 = Vec3(0, 0, 1).to_cross_product_matrix()
#dynamics are R*(I + |_del_|) * vel
#print(self._state_m.vel.to_array())
grad_d0 = self._state_m.att.to_rotation_matrix(
) * d0 * self._state_m.vel.to_array() * dt
grad_d1 = self._state_m.att.to_rotation_matrix(
) * d1 * self._state_m.vel.to_array() * dt
grad_d2 = self._state_m.att.to_rotation_matrix(
) * d2 * self._state_m.vel.to_array() * dt
A_pos_att = np.column_stack((grad_d0, grad_d1, grad_d2))
#velocity to velocity = Rot(omega)_T
A_velvel = gyroImu.to_cross_product_matrix().T
#velocity from attitude error = -g*(-[del x])*R_T * e3
grad_d0 = 9.81 * d0 * self._state_m.att.to_rotation_matrix().T * Vec3(
0, 0, 1).to_array() * dt
grad_d1 = 9.81 * d1 * self._state_m.att.to_rotation_matrix().T * Vec3(
0, 0, 1).to_array() * dt
grad_d2 = 9.81 * d2 * self._state_m.att.to_rotation_matrix().T * Vec3(
0, 0, 1).to_array() * dt
A_velatt = np.column_stack((grad_d0, grad_d1, grad_d2))
#att error from att error. this is from the covariance update paper
gyroCrossRod = gyroImu.to_cross_product_matrix() / 2 * dt
mat = np.eye(3) + gyroCrossRod.T + gyroCrossRod.T * gyroCrossRod.T / 2
A_attatt = mat
#create A matrix
A = np.block([[A_pos, A_posvel, A_pos_att],
[np.zeros((3, 3)), A_velvel, A_velatt],
[np.zeros((3, 3)),
np.zeros((3, 3)), A_attatt]])
return A
def run(self, dt, thrustCmd):
oldSpeed = self._speed
#we don't allow negative speeds
if thrustCmd < 0:
thrustCmd = 0
#simulate_2CC as a first order system.
# we correct for the body's angular velocity
speedCommand = np.sqrt(thrustCmd/self._speedSqrToThrust)
if oldSpeed is None:
oldSpeed = speedCommand
self._speed = speedCommand
else:
if self._timeConst == 0:
c = 0
else:
c = np.exp(-dt/self._timeConst)
self._speed = c*self._speed + (1-c)*speedCommand
#saturate _speed:
if (self._speed) > self._maxSpeed:
self._speed = self._maxSpeed
if (self._speed) < self._minSpeed:
self._speed = self._minSpeed
#aerodynamic force & moment:
self._angularMomentum = self._speed*self._inertia*self._rotAxis
self._thrust = self._speedSqrToThrust*self._speed**2*self._thrustAxis
self._torque = Vec3(0,0,0)
#aero torque:
self._torque += -self._speedSqrToTorque*self._speed*np.abs(self._speed)*self._rotAxis
#torque from thrust acting at distance from com:
self._torque += self._position.cross(self._thrust)
#add moment due to acceleration of propeller:
angularAcceleration = (self._speed - oldSpeed)/dt
self._torque -= angularAcceleration*self._inertia*self._rotAxis
self._powerConsumptionInstantaneous = self._speed*self._torque.norm2()
return
class IMU: #earth's magnetic field vector in earth coordinates, assumed to be pointing in y _refMag = Vec3(0,1,0) def __init__(self, accStd, gyroStd, magStd,test): #assert accStd.shape == (3,1); assert gyroStd.shape == (3,1); assert magStd.shape == (3,1) #print(accStd) self._accStd = accStd self._gyroStd = gyroStd self._magStd= magStd self._test = test #print(self._ref_mag) def get_imu_measurements(self, acc, att, omega): if self._test: accNoise = Vec3(0,0,0) gyroNoise = Vec3(0,0,0) magNoise = Vec3(0,0,0) else: #generate noise based on sensor noise model accNoise = Vec3(np.random.normal()*self._accStd[0],np.random.normal()*self._accStd[1],np.random.normal()*self._accStd[2]) gyroNoise = Vec3(np.random.normal()*self._gyroStd[0],np.random.normal()*self._gyroStd[1],np.random.normal()*self._gyroStd[2]) magNoise = Vec3(np.random.normal()*self._magStd[0],np.random.normal()*self._magStd[1],np.random.normal()*self._magStd[2]) #add gravity acceleration, which is measured by IMU, and rotate earth frame acceleration to body frame acc = att.inverse()*(acc + Vec3(0,0,9.81)) #add noise to true state to obtain measurement acc += accNoise omega += gyroNoise mag = att.inverse()*self._refMag + magNoise #return measurement tuples return acc, omega, mag
def get_imu_measurements(self, acc, att, omega): if self._test: accNoise = Vec3(0,0,0) gyroNoise = Vec3(0,0,0) magNoise = Vec3(0,0,0) else: #generate noise based on sensor noise model accNoise = Vec3(np.random.normal()*self._accStd[0],np.random.normal()*self._accStd[1],np.random.normal()*self._accStd[2]) gyroNoise = Vec3(np.random.normal()*self._gyroStd[0],np.random.normal()*self._gyroStd[1],np.random.normal()*self._gyroStd[2]) magNoise = Vec3(np.random.normal()*self._magStd[0],np.random.normal()*self._magStd[1],np.random.normal()*self._magStd[2]) #add gravity acceleration, which is measured by IMU, and rotate earth frame acceleration to body frame acc = att.inverse()*(acc + Vec3(0,0,9.81)) #add noise to true state to obtain measurement acc += accNoise omega += gyroNoise mag = att.inverse()*self._refMag + magNoise #return measurement tuples return acc, omega, mag
# Define the position controller
#==============================================================================
disablePositionControl = False
posCtrlNatFreq = 1.5 # rad/s
posCtrlDampingRatio = 0.7 # -
#==============================================================================
#Create empty dict for storing quadcopter objects
#==============================================================================
robotDict = {}
nLighthouses = 1
nAnchors = 1
nRobots = 1
#initial locations
lhLocations = [Vec3(0, 0, 0)]
anchorLocations = [Vec3(0.5, 0.5, 0)]
robotLocations = [Vec3(0, 1, 0)]
#==============================================================================
# initialize all quadcopters:
#==============================================================================
for i in range(0, nLighthouses + nAnchors + nRobots):
#determine quadcopter type
if nLighthouses > 0:
droneType = DroneType.lighthouse_drone
nLighthouses -= 1
initPos = lhLocations[
nLighthouses] #traverses the initial pos list backwards
elif nAnchors > 0: