From 2ea95e377a1f57cee0a80ea3886cb229dbf7e9ab Mon Sep 17 00:00:00 2001 From: Tim Trueman Date: Sun, 29 Aug 2010 15:00:34 -0700 Subject: [PATCH] Dead reckoning EKF work --- ekf.py | 53 ++++++++++++++++++++++++++++++++++++++++++++++++ fgo.py | 6 +++--- geonavigation.py | 13 +++++++++++- gps.py | 50 +++++++++++++++++++++++++++++++++++++-------- shumai.py | 18 ++++++++++++---- 5 files changed, 123 insertions(+), 17 deletions(-) diff --git a/ekf.py b/ekf.py index 4ab728e..9d08f61 100644 --- a/ekf.py +++ b/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 diff --git a/fgo.py b/fgo.py index 3810b93..58c3048 100644 --- a/fgo.py +++ b/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 diff --git a/geonavigation.py b/geonavigation.py index d96f8c5..fdd33a4 100644 --- a/geonavigation.py +++ b/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 \ No newline at end of file diff --git a/gps.py b/gps.py index b60d9dc..8769deb 100644 --- a/gps.py +++ b/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 diff --git a/shumai.py b/shumai.py index bfe20dc..ace4f27 100644 --- a/shumai.py +++ b/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'