Ejemplos de quat2Reb en Python

Ejemplo n.º 1

Archivo: accelerometer.py Proyecto: Georacer/last_letter

	def iterate(self,states):
		Reb = quat2Reb(states.pose.orientation)
		
		rw_noise = np.random.normal(0,self.p0,3)
		self.randomwalk.x +=self.dt*rw_noise[0]
		self.randomwalk.y +=self.dt*rw_noise[1]
		self.randomwalk.z +=self.dt*rw_noise[2]
		
		self.bias.x = self.bb[0] + self.kb[0] * (self.temp - 298.15)
		self.bias.y = self.bb[1] + self.kb[1] * (self.temp - 298.15)
		self.bias.z = self.bb[1] + self.kb[1] * (self.temp - 298.15)
		
		self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
		self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
		self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)

		avel = Vector2Array(states.velocity.angular)
		aveldot = Vector2Array(states.acceleration.angular)
		missal = np.cross(avel,np.cross(avel,self.cp.T)) + np.cross(aveldot,self.cp.T)
		
		self.measurement.axis.x = (1+self.scale.x)*(states.acceleration.linear.x/self.grav + missal[0][0] + self.grav*Reb[2][0]) + self.bias.x + self.randomwalk.x
		self.measurement.axis.y = (1+self.scale.y)*(states.acceleration.linear.y/self.grav + missal[0][1] + self.grav*Reb[2][1]) + self.bias.y + self.randomwalk.y
		self.measurement.axis.z = (1+self.scale.z)*(states.acceleration.linear.z/self.grav + missal[0][2] + self.grav*Reb[2][2]) + self.bias.z + self.randomwalk.z

		self.measurement.axis.x = saturation(floor(self.measurement.axis.x/self.resolution)*self.resolution,-self.maxvalue*self.grav, self.maxvalue*self.grav)
		self.measurement.axis.y = saturation(floor(self.measurement.axis.y/self.resolution)*self.resolution,-self.maxvalue*self.grav, self.maxvalue*self.grav)
		self.measurement.axis.z = saturation(floor(self.measurement.axis.z/self.resolution)*self.resolution,-self.maxvalue*self.grav, self.maxvalue*self.grav)

		self.tempOffset = self.thermalMass.step(self.airspeed,self.dt)
		self.measurement.temperature = self.temp + self.tempOffset

Ejemplo n.º 2

Archivo: magnetometer.py Proyecto: gitdetector/last_letter

    def iterate(self, states):

        Reb = quat2Reb(states.pose.orientation)
        Mb, dec = getMag(states.geoid.latitude, states.geoid.longitude,
                         states.geoid.altitude, Reb)

        nm_noise = np.random.normal(0, self.std_coeff, 3)

        self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
        self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
        self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)

        self.measurement.axis.x = (1 + self.scale.x) * Mb.x + nm_noise[0]
        self.measurement.axis.y = (1 + self.scale.y) * Mb.y + nm_noise[1]
        self.measurement.axis.z = (1 + self.scale.z) * Mb.z + nm_noise[2]

        self.measurement.axis.x = floor(
            self.measurement.axis.x / self.precision) * self.precision
        self.measurement.axis.y = floor(
            self.measurement.axis.y / self.precision) * self.precision
        self.measurement.axis.z = floor(
            self.measurement.axis.z / self.precision) * self.precision

        self.measurement.temperature = self.temp

        self.tempOffset = self.thermalMass.step(self.airspeed, self.dt)
        self.measurement.temperature = self.temp + self.tempOffset

Ejemplo n.º 3

Archivo: gyroscope.py Proyecto: Georacer/last_letter

	def iterate(self,states):
		Reb = quat2Reb(states.pose.orientation)

		rw_noise = np.random.normal(0,self.p0,3)
		self.randomwalk.x +=self.dt*rw_noise[0]
		self.randomwalk.y +=self.dt*rw_noise[1]
		self.randomwalk.z +=self.dt*rw_noise[2]
		
		self.bias.x = self.bb[0] + self.kb[0] * (self.temp - 298.15)
		self.bias.y = self.bb[1] + self.kb[1] * (self.temp - 298.15)
		self.bias.z = self.bb[1] + self.kb[1] * (self.temp - 298.15)
		
		self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
		self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
		self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)

		self.measurement.axis.x = (1+self.scale.x)*states.velocity.angular.x + self.bias.x + self.randomwalk.x
		self.measurement.axis.y = (1+self.scale.y)*states.velocity.angular.y + self.bias.y + self.randomwalk.y
		self.measurement.axis.z = (1+self.scale.z)*states.velocity.angular.z + self.bias.z + self.randomwalk.z

		self.measurement.axis.x = saturation(floor(self.measurement.axis.x/self.resolution)*self.resolution,-self.maxvalue, self.maxvalue)
		self.measurement.axis.y = saturation(floor(self.measurement.axis.y/self.resolution)*self.resolution,-self.maxvalue, self.maxvalue)
		self.measurement.axis.z = saturation(floor(self.measurement.axis.z/self.resolution)*self.resolution,-self.maxvalue, self.maxvalue)
		
		self.tempOffset = self.thermalMass.step(self.airspeed,self.dt)
		self.measurement.temperature = self.temp + self.tempOffset

Ejemplo n.º 4

Archivo: fdmUDPSend.py Proyecto: Akshath-Singhal/last_letter

def accel_callback(accel):

    global fdm, stateStorage
    Reb = quat2Reb(stateStorage.pose.orientation)
    accx = (accel.x - 9.80665 * Reb[2][0])
    accy = (accel.y - 9.80665 * Reb[2][1])
    accz = (accel.z - 9.80665 * Reb[2][2])
    fdm.set('A_X_pilot', accx, units='mpss')
    fdm.set('A_Y_pilot', accy, units='mpss')
    fdm.set('A_Z_pilot', accz, units='mpss')

Ejemplo n.º 5

Archivo: barometer.py Proyecto: Georacer/last_letter

	def iterate(self,states):
		Reb = quat2Reb(states.pose.orientation)
		bar_noise = np.random.normal(0,self.std_coeff,1)
		barpos = states.pose.position.z+np.dot(Reb[2][0:3],self.cp)
		
		self.measurement.pressure = self.bar_true
		self.measurement.pressure = saturation(floor(self.measurement.pressure/self.resolution)*self.resolution,self.minvalue, self.maxvalue)

		self.tempOffset = self.thermalMass.step(self.airspeed,self.dt)
		self.measurement.temperature = self.temp + self.tempOffset

Ejemplo n.º 6

Archivo: fdmUDPSend.py Proyecto: tridge/last_letter

def accel_callback(accel):

	global fdm, stateStorage
	Reb = quat2Reb(stateStorage.pose.orientation)
	accx = (accel.x - 9.80665*Reb[2][0])
	accy = (accel.y - 9.80665*Reb[2][1])
	accz = (accel.z - 9.80665*Reb[2][2])
	fdm.set('A_X_pilot', accx, units='mpss')
	fdm.set('A_Y_pilot', accy, units='mpss')
	fdm.set('A_Z_pilot', accz, units='mpss')

Ejemplo n.º 7

Archivo: barometer.py Proyecto: gitdetector/last_letter

    def iterate(self, states):
        Reb = quat2Reb(states.pose.orientation)
        bar_noise = np.random.normal(0, self.std_coeff, 1)
        barpos = states.pose.position.z + np.dot(Reb[2][0:3], self.cp)

        self.measurement.pressure = self.bar_true
        self.measurement.pressure = saturation(
            floor(self.measurement.pressure / self.resolution) *
            self.resolution, self.minvalue, self.maxvalue)

        self.tempOffset = self.thermalMass.step(self.airspeed, self.dt)
        self.measurement.temperature = self.temp + self.tempOffset

Ejemplo n.º 8

    def iterate(self, states):
        Reb = quat2Reb(states.pose.orientation)

        rw_noise = np.random.normal(0, self.p0, 3)
        self.randomwalk.x += self.dt * rw_noise[0]
        self.randomwalk.y += self.dt * rw_noise[1]
        self.randomwalk.z += self.dt * rw_noise[2]

        self.bias.x = self.bb[0] + self.kb[0] * (self.temp - 298.15)
        self.bias.y = self.bb[1] + self.kb[1] * (self.temp - 298.15)
        self.bias.z = self.bb[1] + self.kb[1] * (self.temp - 298.15)

        self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
        self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
        self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)

        avel = Vector2Array(states.velocity.angular)
        aveldot = Vector2Array(states.acceleration.angular)
        missal = np.cross(avel, np.cross(avel, self.cp.T)) + np.cross(
            aveldot, self.cp.T)

        self.measurement.axis.x = (1 + self.scale.x) * (
            states.acceleration.linear.x / self.grav + missal[0][0] +
            self.grav * Reb[2][0]) + self.bias.x + self.randomwalk.x
        self.measurement.axis.y = (1 + self.scale.y) * (
            states.acceleration.linear.y / self.grav + missal[0][1] +
            self.grav * Reb[2][1]) + self.bias.y + self.randomwalk.y
        self.measurement.axis.z = (1 + self.scale.z) * (
            states.acceleration.linear.z / self.grav + missal[0][2] +
            self.grav * Reb[2][2]) + self.bias.z + self.randomwalk.z

        self.measurement.axis.x = saturation(
            floor(self.measurement.axis.x / self.resolution) * self.resolution,
            -self.maxvalue * self.grav, self.maxvalue * self.grav)
        self.measurement.axis.y = saturation(
            floor(self.measurement.axis.y / self.resolution) * self.resolution,
            -self.maxvalue * self.grav, self.maxvalue * self.grav)
        self.measurement.axis.z = saturation(
            floor(self.measurement.axis.z / self.resolution) * self.resolution,
            -self.maxvalue * self.grav, self.maxvalue * self.grav)

        self.tempOffset = self.thermalMass.step(self.airspeed, self.dt)
        self.measurement.temperature = self.temp + self.tempOffset

Ejemplo n.º 9

Archivo: altimeter.py Proyecto: Georacer/last_letter

	def iterate(self,states):
		Reb = quat2Reb(states.pose.orientation)
		
		if abs(Reb[2][2])<=1e-4:
			self.measurement.value=self.maxvalue
		else:
			self.measurement.value=saturation((-states.pose.position.z-np.dot(Reb[2][0:3],self.cp)/Reb[2][2]),self.minvalue,self.maxvalue)
		
		rf_noise = np.random.normal(0,self.std_coeff,1)
		if self.measurement.value<=self.minvalue:
			rf_noise = np.random.normal(0,self.minvalue/3.0,1)
		if self.measurement.value>=self.maxvalue-1:
			rf_noise = np.random.normal(0,self.maxvalue/3.0,1)
			

		self.measurement.value +=rf_noise[0]
		self.measurement.value = max(self.minvalue,floor(self.measurement.value/self.resolution)*self.resolution)
		
		self.tempOffset = self.thermalMass.step(self.airspeed,self.dt)
		self.measurement.temperature = self.temp + self.tempOffset

Ejemplo n.º 10

    def iterate(self, states):
        Reb = quat2Reb(states.pose.orientation)

        rw_noise = np.random.normal(0, self.p0, 3)
        self.randomwalk.x += self.dt * rw_noise[0]
        self.randomwalk.y += self.dt * rw_noise[1]
        self.randomwalk.z += self.dt * rw_noise[2]

        self.bias.x = self.bb[0] + self.kb[0] * (self.temp - 298.15)
        self.bias.y = self.bb[1] + self.kb[1] * (self.temp - 298.15)
        self.bias.z = self.bb[1] + self.kb[1] * (self.temp - 298.15)

        self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
        self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
        self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)

        self.measurement.axis.x = (
            1 + self.scale.x
        ) * states.velocity.angular.x + self.bias.x + self.randomwalk.x
        self.measurement.axis.y = (
            1 + self.scale.y
        ) * states.velocity.angular.y + self.bias.y + self.randomwalk.y
        self.measurement.axis.z = (
            1 + self.scale.z
        ) * states.velocity.angular.z + self.bias.z + self.randomwalk.z

        self.measurement.axis.x = saturation(
            floor(self.measurement.axis.x / self.resolution) * self.resolution,
            -self.maxvalue, self.maxvalue)
        self.measurement.axis.y = saturation(
            floor(self.measurement.axis.y / self.resolution) * self.resolution,
            -self.maxvalue, self.maxvalue)
        self.measurement.axis.z = saturation(
            floor(self.measurement.axis.z / self.resolution) * self.resolution,
            -self.maxvalue, self.maxvalue)

        self.tempOffset = self.thermalMass.step(self.airspeed, self.dt)
        self.measurement.temperature = self.temp + self.tempOffset

Ejemplo n.º 11

Archivo: magnetometer.py Proyecto: Georacer/last_letter

	def iterate(self,states):

		Reb = quat2Reb(states.pose.orientation)
		Mb,dec=getMag(states.geoid.latitude,states.geoid.longitude,states.geoid.altitude,Reb)

		nm_noise = np.random.normal(0,self.std_coeff,3)
		
		self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
		self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
		self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)

		self.measurement.axis.x=(1+self.scale.x)*Mb.x+nm_noise[0]
		self.measurement.axis.y=(1+self.scale.y)*Mb.y+nm_noise[1]
		self.measurement.axis.z=(1+self.scale.z)*Mb.z+nm_noise[2]

		self.measurement.axis.x = floor(self.measurement.axis.x/self.precision)*self.precision
		self.measurement.axis.y = floor(self.measurement.axis.y/self.precision)*self.precision
		self.measurement.axis.z = floor(self.measurement.axis.z/self.precision)*self.precision

		self.measurement.temperature = self.temp
		
		self.tempOffset = self.thermalMass.step(self.airspeed,self.dt)
		self.measurement.temperature = self.temp + self.tempOffset

Ejemplo n.º 12

Archivo: altimeter.py Proyecto: gitdetector/last_letter

    def iterate(self, states):
        Reb = quat2Reb(states.pose.orientation)

        if abs(Reb[2][2]) <= 1e-4:
            self.measurement.value = self.maxvalue
        else:
            self.measurement.value = saturation(
                (-states.pose.position.z -
                 np.dot(Reb[2][0:3], self.cp) / Reb[2][2]), self.minvalue,
                self.maxvalue)

        rf_noise = np.random.normal(0, self.std_coeff, 1)
        if self.measurement.value <= self.minvalue:
            rf_noise = np.random.normal(0, self.minvalue / 3.0, 1)
        if self.measurement.value >= self.maxvalue - 1:
            rf_noise = np.random.normal(0, self.maxvalue / 3.0, 1)

        self.measurement.value += rf_noise[0]
        self.measurement.value = max(
            self.minvalue,
            floor(self.measurement.value / self.resolution) * self.resolution)

        self.tempOffset = self.thermalMass.step(self.airspeed, self.dt)
        self.measurement.temperature = self.temp + self.tempOffset