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
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
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
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
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
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
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
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
