From 294a1b49cc98b8f3dbb43ce596551ac8f213bee5 Mon Sep 17 00:00:00 2001 From: Tim Trueman Date: Mon, 18 Oct 2010 02:16:32 -0700 Subject: [PATCH] Dead reckoning working with wind estimation, need to tweak the gains though --- ekf.py | 19 +++++++++++-------- gps.py | 8 ++++---- shumai.py | 9 ++++----- truthdata.py | 1 + 4 files changed, 20 insertions(+), 17 deletions(-) diff --git a/ekf.py b/ekf.py index 9d08f61..6e5d251 100644 --- a/ekf.py +++ b/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 diff --git a/gps.py b/gps.py index 8769deb..e98d88a 100644 --- a/gps.py +++ b/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 diff --git a/shumai.py b/shumai.py index ace4f27..28c59ee 100644 --- a/shumai.py +++ b/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 = '