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

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

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