Dead reckoning EKF work

ekf.py

@@ -1,5 +1,6 @@
 from math import cos, sin, tan, atan, atan2, isnan, pi, degrees, radians, sqrt
 from numpy import matrix
+from scipy import linalg
 from datetime import datetime
 import logging
 import logging.config
@@ -9,6 +10,9 @@
 import logging.config
 from display import Display
 
+from truthdata import TruthData
+
+TD = TruthData()
 display = Display()
 
 logging.config.fileConfig("logging.conf")
@@ -294,3 +298,52 @@ def estimate_heading(self, bx, by, bz, phi, theta, q, r, dt):
         logger.info("Heading %f" % self.psi_estimate)
         display.register_scalars({"Psi":degrees(self.psi_estimate)})
         return self.psi_estimate
+
+class PositionObserver:
+    def __init__(self):
+        self.Qxt = 0.00025 # noise due to heading error & airspeed
+        self.Qot = 0.00025 # on track noise due to airspeed
+        self.Wn_est = 0.0
+        self.We_est = 0.0
+        self.A = matrix("0,0,-1,0; 0,0,0,-1; 0,0,0,0; 0,0,0,0")
+        self.I = matrix("1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1")
+        self.P = matrix("1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1")
+        self.Q = matrix("1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1")
+        self.L = matrix("1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1")
+        self.C = matrix("1,0,0,0;0,1,0,0")
+        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
+        theta = TD.PITCH
+        Vair = TD.AIRSPEED
+        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,
+                                        0,
+                                        0))
+        self.X = self.X+X_dot*dt
+
+        self.A = matrix("0,0,-1,0;0,0,0,-1;0,0,0,0;0,0,0,0")
+
+        P_dot = self.A*self.P + self.P*self.A.transpose() + self.Q
+        self.P = self.P + P_dot*dt
+
+        self.C = matrix("1,0,0,0;0,1,0,0")
+
+        self.L = self.P*self.C.transpose()*linalg.inv(self.R+self.C*self.P*self.C.transpose())
+        #To invert the matrix it must not be singular ie have a det()=0
+
+        self.P = (self.I-self.L*self.C)*self.P
+
+        z = matrix("%f;%f" % (GPS_Pn,GPS_Pe))
+        h = matrix("%f;%f" % (self.X[0,0],self.X[1,0]))
+
+        self.X = self.X + self.L*(z-h)
+        return self.X

fgo.py

@@ -29,7 +29,7 @@ class WindObserver:
     ''' Ryan Beall is awesome.
     '''
     def __init__(self):
-        self.k = 0.025
+        self.k = 0.05
         self.Wn_fgo = 0.0
         self.We_fgo = 0.0
 
@@ -49,7 +49,7 @@ def estimate(self, theta, psi, Vair, ground_speed, course_over_ground, dt):
         wind_east_error = (Vair * sin(psi) * cos(theta)) - (ground_speed * sin(course_over_ground)) - self.We_fgo
         self.Wn_fgo += self.k * wind_north_error
         self.We_fgo += self.k * wind_east_error
-        display.register_scalars({"Wn error": wind_north_error, "We error": wind_east_error, "Wn_fgo": self.Wn_fgo, "We_fgo": self.We_fgo, "cos(psi)": cos(psi), "cos(theta)": cos(theta), "sin(psi)": sin(psi), "cos(cog)": cos(course_over_ground), "sin(cog)": sin(course_over_ground), "wind_north_error": wind_north_error, "wind_east_error": wind_east_error}, "Wind FGO")
-        wind_direction = degrees(atan2(self.We_fgo,self.Wn_fgo)) + 180.0
+        display.register_scalars({"CoG":course_over_ground,"psi":psi,"Wn error": wind_north_error, "We error": wind_east_error, "Wn_fgo": self.Wn_fgo, "We_fgo": self.We_fgo, "cos(psi)": cos(psi), "cos(theta)": cos(theta), "sin(psi)": sin(psi), "cos(cog)": cos(course_over_ground), "sin(cog)": sin(course_over_ground), "wind_north_error": wind_north_error, "wind_east_error": wind_east_error}, "Wind FGO")
+        wind_direction = degrees(atan2(self.We_fgo,self.Wn_fgo))
         wind_velocity = sqrt(self.We_fgo**2 + self.Wn_fgo**2) / 0.5144444444 # convert from m/s to knots
         return wind_direction, wind_velocity

geonavigation.py

@@ -1,7 +1,18 @@
-from math import sin, cos, atan2, degrees
+from math import sin, cos, atan2, degrees, pi
 
 def gps_coords_to_heading(lat1, lon1, lat2, lon2):
     # lat = lat2-lat1
     lon = lon2 - lon1
     targetHeading = degrees(atan2(sin(lon) * cos(lat2), cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon)))
     return (targetHeading + 360) % 360
+
+def heading(lat1,lon1,lat2,lon2):
+    #calculated intial course to second waypoint
+    heading = atan2(cos(lat1*(pi/180))*sin(lat2*(pi/180))-sin(lat1*(pi/180))*cos(lat2*(pi/180))*cos((lon2-lon1)*(pi/180)), sin((lon2-lon1)*(pi/180))*cos(lat2*(pi/180)))
+    heading = -(heading*(180/pi)-90)
+    if heading <0:
+        heading = heading +360
+    if heading >180:
+        heading = heading -360
+    heading = heading*(pi/180)
+    return heading

gps.py

@@ -1,10 +1,12 @@
-from math import radians, sin, sqrt, atan2, cos
+from math import radians, sin, sqrt, atan2, cos, pi
 from time import time
 from utils import safe_tangent, wrap
 from truthdata import TruthData
-from geonavigation import gps_coords_to_heading
+from geonavigation import heading
+from display import Display
 
 TD = TruthData()
+display = Display()
 
 class EmulatedXplaneGPS:
     ''' Emulates the behavior of GPS via X-Plane truth data
@@ -14,25 +16,55 @@ def __init__(self, delay=1.1, Hz=2.0):
         self.delay = delay
         self.update_dt = 1.0/Hz
         self.last_update = time() + delay
-        self.speed_over_ground = 0
-        self.course_over_ground = 0
-        self.latitude = 0
-        self.longitude = 0
-        self.altitude = 0
-        self.last_latitude = 0
-        self.last_longitude = 0
+        self.speed_over_ground = 0.0
+        self.course_over_ground = 0.0
+        self.latitude = 0.0
+        self.longitude = 0.0
+        self.altitude = 0.0
+        self.last_latitude = 0.0
+        self.last_longitude = 0.0
+        self.home_latitude = 0.0
+        self.home_longitude = 0.0
         self.cog = []
         self.sog = []
         self.lat = []
         self.lon = []
         self.alt = []
 
+    def relative_gps(self):
+        lat = self.home_latitude - TD.LATITUDE
+        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")
+        Pn = cos(lamb)*d
+        Pe = sin(lamb)*d
+        return (Pn, Pe)
+
+    def dist_to_wpt(self,lat1,lon1,lat2,lon2):
+        # calculates great route circle in between waypoints
+        R = 6371 # earth's radius
+        #distance calc
+        delta_lat = (lat2 - lat1)#(pi/180);
+        delta_lon = (lon2 - lon1)#*(pi/180);
+        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
+
+
+
     def update(self, dt):
         self.cog.append(TD.COURSEOVERGROUND)
         self.sog.append(TD.SPEEDOVERGROUND)
         self.lat.append(TD.LATITUDE)
         self.lon.append(TD.LONGITUDE)
         self.alt.append(TD.ALTITUDE)
+        if self.home_latitude == 0.0 and self.home_longitude == 0.0:
+            self.home_latitude = TD.LATITUDE
+            self.home_longitude = TD.LONGITUDE
         now = time()
         if now - self.last_update > self.update_dt:
             self.last_update = now

shumai.py

@@ -10,7 +10,7 @@
 import logging.config
 import csv
 from display import Display
-from ekf import AttitudeObserver
+from ekf import AttitudeObserver, PositionObserver
 from vgo import HeadingObserver
 from fgo import AltitudeObserver, WindObserver
 from gps import EmulatedXplaneGPS
@@ -72,11 +72,14 @@ 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
+        self.Vair = 0.0
+        self.Wn = 0.0
+        self.We = 0.0
         self.attitude_observer = AttitudeObserver()
         self.heading_observer = HeadingObserver()
         self.altitude_observer = AltitudeObserver()
         self.wind_observer = WindObserver()
+        self.position_observer = PositionObserver()
         # Comming soon:
         # self.navigation_observer = NavigationObserver()
         self.imu = imu
@@ -102,10 +105,17 @@ def loop(self):
                                   "gps_alt": gps_data['altitude'],
                                   "gps_sog": gps_data['speed_over_ground'],}, "Sensors")
         altitude = self.altitude_observer.estimate(theta, Vair, gps_data['altitude'], TD.DT)
+
         display.register_scalars({"alt_est": altitude}, "Estimates")
         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, radians(gps_data['course_over_ground']), TD.DT)
-        display.register_scalars({"wind_direction": wind_direction, "wind_velocity": wind_velocity}, "Estimates")
+        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)
+        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")
+        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
         # //SOG[0] = 'data from gps'