simple_plot.plt

set style data lines

set term aqua 1
set title "absolute speed"
plot "data" u 3

set term aqua 2
set title "absolute windspeed"
plot "data" u 4


set term aqua 3
set title "position"
set xrange [-1500:1500]
set yrange [0:3000]
plot "data"

simple_sim.py

#!/usr/bin/env python

import scipy as S
import scipy.linalg as LIN
from scipy.interpolate import interp1d

class Wind(object):
	"""
	A class for wind in meters per second.
	"""
	def __call__(self,location):
		"""
		Returns a vector (2d or 3d, whatever) with the wind at position _location_ and time t

		t in seconds,
		location in meters
		
		returned value in meters/second
		"""
		raise NotImplementedError("Abstract Base Class!")

	def update(t):
		"""
		Advance time by t, override if you have wind that changes with time.
		"""
		pass

class IsotropicWind(Wind):
	def __init__(self,d_vector):
		self.d = S.array(d_vector)
	
	def __call__(self,loc):
		return self.d

####
class Controls(object):
	def __init__(self,initial,min=None,max=None):
		self.vals = S.array(initial)
		self.mins = S.zeros(self.vals.size) if min is None else S.array(min)
		self.maxs = S.ones(self.vals.size) if max is None else S.array(max)
	
	def __call__(self,newvals):
		self.vals[:] = S.array(newvals)
		toohigh = self.vals > self.maxs
		toolow = self.vals < self.mins
		self.vals[toohigh] = self.maxs[toohigh]
		self.vals[toolow] = self.maxs[toolow]

	def __getitem__(self, index):
		return self.vals[index:index+1]

class Drag(object):
	def __init__(self, front, side, tail):
		self.f = front
		self.s = side
		self.t = tail
		self.lin = interp1d([-1.0001,-1.0,0.0,1.0,1.0001], [tail,tail, side, front,front])
	
	def __call__(self,x,heading, velocity, wind):
		"""
			Find the effective headwind, calculate drag from that.
		"""
		effective_wind = wind(x) - velocity
		drag_coeff = 0.0 if S.allclose(effective_wind,0.0) else self.lin(S.dot(effective_wind/LIN.norm(effective_wind), heading))
		drag_force = effective_wind * drag_coeff
		return drag_force

class Engine(object):
	def __init__(self, throtle, impulse):
		"""
		The engine will provide impulse*throtle power. Make sure throtle is in [0.0, 1.0]

		Throtle should be a view from a Control.
		"""
		self.throtle = throtle
		self.impulse = impulse
	
	def __call__(self,x,heading, velocity, wind):
		return heading * self.throtle * self.impulse

class Rudder(object):
	def __init__(self, pedal, maxtorque):
		self.pedal = pedal
		self.maxtorque = maxtorque
	
	def __call__(self,x,heading, velocity, wind):
		return self.pedal * self.maxtorque

class Aircraft(object):
	def __init__(self):
		self.x = S.array([0.0,0.0]) #Position in meters
		self.wind = IsotropicWind(S.array([0.0,0.0])) #1 meter a second wind toward the north.
		self.drag = Drag(0.1,0.5,0.2) #Drag is Newtons / (meters/second) multiply by wind to get drag force
		self.m = 100.0 #Mass in kg
		self.I = 1.0 #WE just assume perfect response to rudder
		self.v = S.zeros(2)
		self.h = S.array([0.0,1.0]) #pointing north.
		#self.rotate(- S.pi/8.0) # Pointing NNE
		
		self.c = Controls([0.0,0.0],[0.0,-1.0], [1.0, 1.0]) # throttle and rudder
		
		self.forces = [self.drag, Engine(self.c[0],1.0)]
		self.torques = [Rudder(self.c[1], 0.01)]

	def get_windspeed(self):
		effective_wind = self.wind(self.x) - self.v
		return LIN.norm(effective_wind)

	def rotate(self, omega):
		rot_matrix = S.array([[S.cos(omega), S.sin(omega)],[-S.sin(omega), S.cos(omega)]])
		self.h = S.dot(rot_matrix, self.h.T).T
		self.h = self.h / LIN.norm(self.h)

	def update(self,t):
		"""
		Update the Aircraft by t time.
		"""
		total_force = sum(map(lambda f:f(self.x,self.h, self.v, self.wind), self.forces))
		total_torque = sum(map(lambda f:f(self.x,self.h, self.v, self.wind), self.torques))

		#linear update, consider runga kutta
		##VELOCITY
		self.v = self.v + t*(total_force/self.m)
		
		##HEADING
		t_omega = t*(total_torque/self.I)[0]
		self.rotate(t_omega)

		#POSITION
		self.x = self.x + t*(self.v)

if __name__ == "__main__":
	myA = Aircraft()
	myA.c([1.0,0.0])

	stepsize = 0.1
	simtime = 1200 # 20 minutes

	for t in S.arange(0.0,simtime, stepsize):
		if t>300:
			myA.c([1.0, S.sin(t/60.0)])#we slowly turn left and right
		myA.update(stepsize)
		print myA.x[0], myA.x[1], LIN.norm(myA.v), myA.get_windspeed()