コード例 #1
0
 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)
コード例 #2
0
   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)
コード例 #3
0
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()
コード例 #4
0
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()
コード例 #5
0
 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
コード例 #6
0
 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
コード例 #7
0
 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))
コード例 #8
0
 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))
コード例 #9
0
 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]
コード例 #10
0
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)
コード例 #11
0
ファイル: Motor.py プロジェクト: henryleinhos/pythonQuadSim
 def commandAngle(self, ang = 0,axis = np.array([[1,0,0]]).T ):
     self.m_Q_v = AQ.quatFromAxisAngle(axis,ang)
コード例 #12
0
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)
コード例 #13
0
    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]
コード例 #14
0
ファイル: TriRotor.py プロジェクト: thehammstr/pythonQuadSim
 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]
コード例 #15
0
    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]