Dead reckoning working with wind estimation, need to tweak the gains …

…though

ekf.py

@@ -8,7 +8,6 @@
 from utils import safe_tangent, wrap, GRAVITY
 import logging
 import logging.config
-from display import Display
 
 from truthdata import TruthData
 
@@ -132,13 +131,13 @@ def check_for_divergence(self):
         if abs(degrees(self.phi)) > 90:
             if self.roll_is_departed == False:
                 self.roll_is_departed = True
-                logger.critical("Roll has departed.")
+                #logger.critical("Roll has departed.")
         else:
             self.roll_is_departed = False
         if abs(degrees(self.theta)) > 90:
             if self.pitch_is_departed == False:
                 self.pitch_is_departed = True
-                logger.critical("Pitch has departed.")
+                #logger.critical("Pitch has departed.")
         else:
             self.pitch_is_departed = False
 
@@ -314,18 +313,20 @@ def __init__(self):
         self.X = matrix("0;0;0;0")
         self.R = matrix("1,0;0,1")
 
-    def estimate(self, Vair, theta, psi, GPS_Pn, GPS_Pe, Wn_est, We_est, dt):
-        psi = TD.HEADING
+    def estimate(self, Vair, theta, psi, GPS_Pn, GPS_Pe, dt):
+        # CHECK UNITS
+        psi = wrap(TD.HEADING)
         theta = TD.PITCH
-        Vair = TD.AIRSPEED
+        Vair = TD.TRUEAIRSPEED * 1.68780986 # m/s to ft/s
+        display.register_scalars({"Vair":Vair,"psi":psi,"theta":theta,"dt":dt},"Dead reckoning")
         Qpn = (self.Qot*abs(cos(psi))) + (self.Qxt*abs(sin(psi)))
         Qpe = (self.Qot*abs(sin(psi))) + (self.Qxt*abs(cos(psi)))
         Qwn = .00001
         Qwe = .00001
         self.Q = matrix("%f,0,0,0; 0,%f,0,0; 0,0,%f,0; 0,0,0,%f" % (Qpn, Qpe, Qwn, Qwe))
 
-        X_dot = matrix("%f;%f;%f;%f" % (Vair*cos(psi)*cos(theta) - Wn_est,
-                                        Vair*sin(psi)*cos(theta) - We_est,
+        X_dot = matrix("%f;%f;%f;%f" % (Vair*cos(psi)*cos(theta) - self.Wn_est,
+                                        Vair*sin(psi)*cos(theta) - self.We_est,
                                         0,
                                         0))
         self.X = self.X+X_dot*dt
@@ -346,4 +347,6 @@ def estimate(self, Vair, theta, psi, GPS_Pn, GPS_Pe, Wn_est, We_est, dt):
         h = matrix("%f;%f" % (self.X[0,0],self.X[1,0]))
 
         self.X = self.X + self.L*(z-h)
+        self.Wn_est, self.We_est = self.X[2,0], self.X[3,0]
+        display.register_scalars({"z-h:0":(z-h)[0,0],"z-h:1":(z-h)[1,0]},"Dead reckoning")
         return self.X

gps.py

@@ -36,8 +36,7 @@ def relative_gps(self):
         lon = self.home_longitude - TD.LONGITUDE
         lamb = heading(self.home_latitude,self.home_longitude, TD.LATITUDE, TD.LONGITUDE)
         d = self.dist_to_wpt(self.home_latitude,self.home_longitude, TD.LATITUDE, TD.LONGITUDE)
-        lamb = radians(lamb)
-        display.register_scalars({"lambda":lamb,"d": d,}, "Dead reckoning")
+        display.register_scalars({"lambda":lamb,"d": d,"home_latitude":self.home_latitude,"home_longitude":self.home_longitude}, "Dead reckoning")
         Pn = cos(lamb)*d
         Pe = sin(lamb)*d
         return (Pn, Pe)
@@ -51,8 +50,9 @@ def dist_to_wpt(self,lat1,lon1,lat2,lon2):
         a = sin((delta_lat/2)*(pi/180))*sin((delta_lat/2)*(pi/180)) + cos(lat1*(pi/180))*cos(lat2*(pi/180)) * sin((delta_lon/2)*(pi/180))*sin((delta_lon/2)*(pi/180))
         c = 2 * atan2(sqrt(a),sqrt(1-a))
         d = R * c #Km
-        #d = d * 3280.8399 #ft
-        return d*1000 #meters
+        d = d * 3280.8399 #ft
+        return d
+        #return d*1000 #meters
 
 
 

shumai.py

@@ -73,8 +73,6 @@ class Shumai:
     def __init__(self, imu, differential_pressure_sensor, static_pressure_sensor, magnetometer, gps):
         self.phi, self.theta, self.psi = 0, 0, 0
         self.Vair = 0.0
-        self.Wn = 0.0
-        self.We = 0.0
         self.attitude_observer = AttitudeObserver()
         self.heading_observer = HeadingObserver()
         self.altitude_observer = AltitudeObserver()
@@ -110,11 +108,11 @@ def loop(self):
         display.register_scalars({"Altitude": altitude - TD.ALTITUDE}, "Performance")
         wind_direction, wind_velocity = self.wind_observer.estimate(theta, psi, Vair, gps_data['speed_over_ground'] * .5144444444, gps_data['course_over_ground'], TD.DT)
         GPS_Pn, GPS_Pe = self.gps.relative_gps()
-        X = self.position_observer.estimate(Vair, theta, psi, GPS_Pn, GPS_Pe, self.Wn, self.We, TD.DT)
+        X = self.position_observer.estimate(Vair, theta, psi, GPS_Pn, GPS_Pe, TD.DT)
         Pn, Pe, Wn, We = X[0,0], X[1,0], X[2,0], X[3,0]
         wind_direction = degrees(atan2(We,Wn))
-        wind_velocity = sqrt(We**2 + Wn**2) / 0.5144444444 # convert from m/s to knots
-        display.register_scalars({"Pn": Pn,"Pe": Pe,"Wn": Wn,"We": We,}, "Dead reckoning")
+        wind_velocity = sqrt(We**2 + Wn**2) # * 0.592483801 # convert from ft/s to knots
+        display.register_scalars({"Pn": Pn,"Pe": Pe,"Wn": Wn,"We": We,"GPS_Pn":GPS_Pn,"GPS_Pe":GPS_Pe}, "Dead reckoning")
         display.register_scalars({"wind_direction": wind_direction, "wind_velocity": wind_velocity}, "Dead reckoning")
         display.register_scalars({"Wdir error": wind_direction - TD.WIND_DIRECTION, "Wvel error": wind_velocity - TD.WIND_VELOCITY}, "Performance")
         # Shoot, I forget what this is, something from Ryan
@@ -156,6 +154,7 @@ def datagramReceived(self, data, (host, port)):
         """
         fmt = '<f'
         TD.AIRSPEED = unpack_from(fmt, data, 9)[0]
+        TD.TRUEAIRSPEED = unpack_from(fmt, data, 9+8)[0]
         TD.AZ = 0 - unpack_from(fmt, data, 9+16+36)[0]
         TD.AX = unpack_from(fmt, data, 9+20+36)[0]
         TD.AY = unpack_from(fmt, data, 9+24+36)[0]

truthdata.py

@@ -31,5 +31,6 @@ def __init__(self):
         self.LATITUDE = 0.0
         self.LONGITUDE = 0.0
         self.AIRSPEED = 0.0
+        self.TRUEAIRSPEED = 0.0
         self.SPEEDOVERGROUND = 0.0
         self.COURSEOVERGROUND = 0.0