From 3300de2c9daa4aaeba86eef8d321913d4bb69532 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 21 Apr 2015 21:33:27 +1000 Subject: [PATCH] autotest: added some gyro and accel noise to copter sim this noise should be kinematically consistent, so will be better for the EKF --- Tools/autotest/pysim/aircraft.py | 16 ++++++++++++++++ Tools/autotest/pysim/multicopter.py | 3 +++ 2 files changed, 19 insertions(+) diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py index a4138057fc..39effb3583 100644 --- a/Tools/autotest/pysim/aircraft.py +++ b/Tools/autotest/pysim/aircraft.py @@ -1,4 +1,5 @@ import math, util, rotmat, time +import random from rotmat import Vector3, Matrix3 class Aircraft(object): @@ -30,6 +31,9 @@ class Aircraft(object): self.time_base = time.time() self.time_now = self.time_base + 100*1.0e-6 + self.gyro_noise = math.radians(0.1) + self.accel_noise = 0.3 + def on_ground(self, position=None): '''return true if we are on the ground''' if position is None: @@ -93,3 +97,15 @@ class Aircraft(object): self.scaled_frame_time *= 1.001 self.last_wall_time = now + + def add_noise(self, throttle): + '''add noise based on throttle level (from 0..1)''' + self.gyro += Vector3(random.gauss(0, 1), + random.gauss(0, 1), + random.gauss(0, 1)) * throttle * self.gyro_noise + self.accel_body += Vector3(random.gauss(0, 1), + random.gauss(0, 1), + random.gauss(0, 1)) * throttle * self.accel_noise + + + diff --git a/Tools/autotest/pysim/multicopter.py b/Tools/autotest/pysim/multicopter.py index 847066ef8b..220fa295e6 100755 --- a/Tools/autotest/pysim/multicopter.py +++ b/Tools/autotest/pysim/multicopter.py @@ -167,6 +167,9 @@ class MultiCopter(Aircraft): # acceleration (ie. real movement), plus gravity self.accel_body = self.dcm.transposed() * (accel_earth + Vector3(0, 0, -self.gravity)) + # add some noise + self.add_noise(thrust / (self.thrust_scale * len(self.motors))) + # new velocity vector self.velocity += accel_earth * delta_time