Commits

James Goppert committed e207133

Added nonlin model.

  • Participants
  • Parent commits 2de7af3

Comments (0)

Files changed (22)

File nonlinear_model/__init__.py

+from .wrapper import Aircraft, Data

File nonlinear_model/aircraft.py

+import numpy as np
+import scipy.integrate
+
+from camflyer_fortran import camflyer as fortran
+from .data import Data
+
+
+class Aircraft(object):
+
+    def __init__(self, tf=10.0):
+        """
+        tf : final time (sec)
+
+        This  initialize the aircraft sim and allocates
+        all of the data structures.
+        """
+
+        process_noise_power = 0.01
+
+        params = {
+            # mass
+            'g_d': 9.8,
+            'm': 1,
+            'J_x': 0.1,
+            'J_y': 0.1,
+            'J_z': 0.1,
+            'J_xz': 0,
+            'rho': 1.225,
+            's': 1.0,
+            # aerodynamics
+            'CL_0': 0.1,
+            'CL_alpha': 0.1,
+            'CL_dmin': 0.2,
+            'CD_dmin': 0.01,
+            'k_CD_CL': 0.01,
+            'CC_beta': -0.1,
+            'Cl_beta': -0.1,
+            'Cl_p': -0.1,
+            'Cl_da': 0.1,
+            'Cm_0': 0.01,
+            'Cm_alpha': -0.1,
+            'Cm_q': -0.1,
+            'Cm_de': 0.1,
+            'Cn_beta': -0.01,
+            'Cn_r': -0.1,
+            'delta_l_tau': 0.1,
+            'delta_r_tau': 0.1,
+            'delta_t_tau': 0.1,
+            'elvn_max': 0.01,
+            # propulsion
+            't_max': 2,
+            # gps
+            'gps_period': 1.0/2,
+            'gps_V_std': 0.1,
+            'gps_V_res': 0.001,
+            'gps_V_max': 100,
+            'gps_p_std': 1.0,
+            'gps_p_res': 0.001,
+            'gps_p_max': 0.01,
+            'gps_h_std': 3.0,
+            'gps_h_res': 0.01,
+            'gps_h_min': -1e6,
+            'gps_h_max': 1e6,
+            # baro
+            'baro_period': 1.0/10,
+            'baro_std': 0.1,
+            'baro_res': 0.01,
+            'baro_min': -1e6,
+            'baro_max': 1e6,
+            # pitot
+            'pitot_period': 1.0/10,
+            'pitot_std': 0.1,
+            'pitot_res': 0.01,
+            'pitot_min': 0,
+            'pitot_max': 100,
+            # mag
+            'mag_period': 1.0/50,
+            'mag_std': 0.1,
+            'mag_res': 0.01,
+            'mag_max': 1e3,
+            # gyro
+            'gyro_period': 1.0/200,
+            'gyro_std': 0.01,
+            'gyro_res': 0.01,
+            'gyro_max': 1e3,
+            # accel
+            'accel_period': 1.0/200,
+            'accel_std': 0.01,
+            'accel_res': 0.01,
+            'accel_max': 1e3,
+        }
+        # f2py likes all lowercase since
+        # fortran is NOT case senstive
+        params = {key.lower(): params[key]
+                  for key in params.keys()}
+
+        # initial input
+        u0 = np.zeros(Data.n_u)
+        u0_p = u0.view(dtype=Data.u_dtype, type=np.recarray)
+        u0_p.delta_t = 0.5
+
+        # simulation time
+        # for noise to have sufficient bandwidth should run sim at
+        # 100*f_max, where f_max is the fastest closed loop pole
+        # frequency, for our system the actuators are 10 Hz and will
+        # likely be the fastest poles so we will simulate at
+        # 1000 Hz
+        dt = 1.0/1000
+        n_t = int((tf-dt)/dt)
+
+        f_dynamics = lambda t, x, u, y, measure, params: \
+            fortran.dynamics(
+                t, x, u_vect=u, y=y,
+                measure=measure, **params)
+
+        # intiialize simulation
+        sim = scipy.integrate.ode(f_dynamics)
+
+        x0 = np.zeros(Data.n_x)
+        x0_p = x0.view(dtype=Data.x_dtype, type=np.recarray)
+
+        x0_p.V_X[0] = 12  # velocity
+        x0_p.a = 1        # quaternion
+        x0_p.x_l = u0_p.delta_l  # actuator positions
+        x0_p.x_r = u0_p.delta_r
+        x0_p.x_t = u0_p.delta_t
+
+        x0[13:16] = u0[0:3]  # set actuators to trim position
+        sim.set_initial_value(x0)
+        fortran.normalize_quaternions(sim.y)
+        sim.set_integrator('dopri5')
+        data = Data(n_t)
+
+        # save data to class
+        self.sim = sim
+        self.data = data
+        self.process_noise_power = process_noise_power
+        self.u0 = u0
+        self.dt = dt
+        self.data = data
+        self.n_t = n_t
+        self.x0_p = x0_p
+        self.u0_p = u0_p
+        self.params = params
+        self.f_dynamics = f_dynamics
+
+    def simulate(self):
+
+        # get class data
+        params = self.params
+        u0 = self.u0
+        sim = self.sim
+        process_noise_power = self.process_noise_power
+        dt = self.dt
+        data = self.data
+        n_t = self.n_t
+        x0_p = self.x0_p
+        u0_p = self.u0_p
+
+        # intialize loop
+        u = u0
+        i = 0
+
+        y_i = np.zeros(Data.n_y, dtype='f8')
+        y_update_i = np.zeros(Data.n_y_update, dtype='bool')
+
+        # intialize random number generator
+        fortran.set_random_seed(np.random.randint(
+            0, 1e6, fortran.get_random_seed_size()))
+        # initialize process noise vector
+        w = np.zeros(len(sim.y), order='F')
+
+        # TODO add wind
+        wind_n = 0
+        wind_e = 0
+        wind_d = 0
+
+        # main simulation loop
+        while True:
+
+            # send parameters to fortran simulation function
+            # fort_params.update({'u_vect': u})
+            sim.set_f_params(u, np.zeros(29), False, params)
+
+            # integrate continuous dynamics
+            sim.integrate(sim.t + dt)
+            if not sim.successful():
+                print 'sim failed'
+                print sim.t, sim.y
+                break
+
+            # add process noise
+            # note we are using the fortran routine
+            # so that we have a consistent seed/
+            # noise generation algorithm, it is
+            # also faster than calling numpy.randn
+            # since it is using a preallocated matrix
+            # TODO is quaternion noise white?
+            fortran.randn(w)
+            w *= process_noise_power
+            sim.set_initial_value(
+                sim.y + process_noise_power*dt*w,
+                sim.t)
+
+            # normalize quaterions
+            fortran.normalize_quaternions(sim.y)
+
+            # take measurements
+            dx, y_update_i = self.f_dynamics(
+                t=sim.t, x=sim.y, u=u, y=y_i,
+                measure=True, params=params)
+
+            # yp_update_i = data.y_update.view(
+            #     dtype=Data.y_update_dtype, type=np.recarray)
+
+            # save data
+            data.set(i, sim.t, sim.y, u, y_i, y_update_i)
+
+            # exit if done
+            if (i+1 >= n_t):
+                break
+            else:
+                i += 1
+
+            # calculate new control
+            y = y_i.view(dtype=Data.y_dtype, type=np.recarray)
+            delta_a = -0.1*y.phi
+            delta_e = -0.1*y.theta
+            delta_t = -0.1*(x0_p.V_X - y.V_T) + u0_p.delta_t
+            # mixing
+            delta_l = delta_e + delta_a + u0_p.delta_l
+            delta_r = delta_e - delta_a + u0_p.delta_r
+
+            # set new input
+            u = [delta_l, delta_r, delta_t, wind_n, wind_e, wind_d]
+
+        data.truncate(i)

File nonlinear_model/autopilot.py

+from .zoh import ZeroOrderHold
+
+
+class Autopilot(object):
+    """
+    This roughly approximates the software running in
+    discrete time on the autopilot.
+    """
+
+    def __init__(self, x0):
+        """
+        Initialize the autopilot states etc.
+        """
+
+        self.dt_observer = 1.0/200
+        self.dt_controller = 1.0/50
+
+        self.zoh_controller = ZeroOrderHold(period=self.dt_controller)
+        self.zoh_observer = ZeroOrderHold(period=self.dt_observer)
+
+        self.x = x0
+
+    def observer(self, t, u, y, zoh_gps):
+        """
+        This function is the observer.
+        """
+        x0 = self.x
+        x = x0 + (x0 + u)*self.dt_observer
+        if zoh_gps.updated:
+            zoh_gps.updated = False
+            x = x + 1*(y - x)
+        self.x = x
+        return x
+
+    def controller(self, t, x):
+        """
+        This function is the controller.
+        """
+        u = -2*x
+        return u
+
+    def update(self, t, y, zoh_gps):
+        """
+        This function simulates the operating system scheduler
+        running on the autopilot.
+        """
+        # controller
+        u = self.zoh_controller.update(t, lambda: self.controller(t, self.x))
+        # observer
+        self.x = self.zoh_observer.update(
+            t, lambda: self.observer(t, u, y, zoh_gps))
+        return u

File nonlinear_model/build.sh

+#!/bin/bash
+f2py -c camflyer.f90 -m camflyer_fortran

File nonlinear_model/camflyer.f90

+      module camflyer
+
+      implicit none
+
+      contains
+
+      subroutine dynamics( &
+                t, &                            ! time
+                x_vect, &                       ! state
+                u_vect, &                       ! input
+                measure, &                      ! do a measurement?
+                g_D, m, J_x, J_y, J_z, J_xz, &  ! mass
+                rho, s, &                       ! aero
+                CL_0, CL_alpha, CL_dmin, &      ! lift
+                CD_dmin, k_CD_CL, &             ! drag
+                CC_beta, &                      ! side force
+                Cl_beta, Cl_p, Cl_da, &         ! rolling 
+                Cm_0, Cm_alpha, Cm_q, Cm_de, &  ! pitching
+                Cn_beta, Cn_r, &                ! yawing
+                delta_l_tau, delta_r_tau, &     ! left/right cntrl
+                elvn_max, &                     ! elevon max deflect.
+                T_max, delta_t_tau, &           ! propulsion
+                gps_period, baro_period, &      ! measurement timing
+                pitot_period, gyro_period, &
+                accel_period, mag_period, &
+                gps_V_res, gps_V_max, &  ! adc params
+                gps_p_res, gps_p_max, &
+                gps_h_res, gps_h_min, gps_h_max, &
+                baro_res, baro_min, baro_max, &
+                pitot_res, pitot_min, pitot_max, &
+                mag_res, mag_max, &
+                gyro_res, gyro_max, &
+                accel_res, accel_max, &
+                gps_V_std, gps_h_std, gps_p_std, & ! meas. noise
+                baro_std, pitot_std, mag_std, &
+                gyro_std, accel_std, &
+                dx, &                           ! state deriv
+                y, y_update)          ! measurement
+
+          ! x: u, v, w, a, b, c, d, P, Q, R, p_N, p_E, h
+          !    delta_l, delta_r, delta_t,
+          !    gyro_bias_x, gyro_bias_y, gyro_bias_z
+          ! u: delta_l_cmd, delta_r_cmd, delta_t,
+          !    wind_N, wind_E, wind_D
+          ! y: V_T, alpha, beta, phi, theta, psi, (aux states)
+          !    t_gps, V_N, V_E, V_D, p_N, p_E, h, (gps)
+          !    t_mag, B_x, B_y, B_z, (mag)
+          !    t_gyro, P, Q, R, (gyro)
+          !    t_accel, a_x, a_y, a_z, (accel)
+          !    t_baro, P_a, (baro)
+          !    t_pitot, P_d (pitot)
+          ! y_update:
+          !    gps, mag, gyro, accel, baro, pitot
+
+          ! input
+          real(8), intent(in) :: t
+          real(8), dimension(19), intent(in) :: x_vect
+          real(8), dimension(6), intent(in) :: u_vect
+          logical, intent(in) :: measure
+          real(8), intent(in) :: g_D, m, J_x, J_y, J_z, J_xz
+          real(8), intent(in) :: rho, s, &
+                CL_0, CL_alpha, CL_dmin, & ! lift curve
+                CD_dmin, k_CD_CL, & ! drag polar
+                CC_beta, & ! beta
+                Cl_beta, Cl_p, Cl_da, & ! rolling
+                Cm_0, Cm_alpha, Cm_q, Cm_de, & ! pitching
+                Cn_beta, Cn_r, & ! yawing
+                delta_l_tau, delta_r_tau, & ! left/right cntrl
+                elvn_max, &                     ! elevon max deflect.
+                T_max, delta_t_tau, &  !propulsion 
+                gps_period, baro_period, &      ! measurement timing
+                pitot_period, gyro_period, &
+                accel_period, mag_period, &
+                gps_V_res, gps_V_max, &  ! adc params
+                gps_p_res, gps_p_max, &
+                gps_h_res, gps_h_min, gps_h_max, &
+                baro_res, baro_min, baro_max, &
+                pitot_res, pitot_min, pitot_max, &
+                mag_res, mag_max, &
+                gyro_res, gyro_max, &
+                accel_res, accel_max, &
+                gps_V_std, gps_h_std, gps_p_std, & ! meas. noise
+                baro_std, pitot_std, mag_std, &
+                gyro_std, accel_std
+
+          ! output
+          real(8), dimension(19), intent(out) :: dx
+          real(8), intent(inout), dimension(29) :: y
+          logical, dimension(6), intent(out) :: y_update
+
+          ! local variables
+          real(8), dimension(3,3) :: C_bw, C_bn
+          real(8) :: p_N, p_E, h, &
+                delta_l, delta_r, delta_t, &
+                delta_l_cmd, delta_r_cmd, delta_t_cmd, &
+                gyro_bias_x, gyro_bias_y, gyro_bias_z, &
+                gam, q_bar, P, Q, R, de, da
+          real(8), dimension(3,3) :: C_nb ! dcm from bodty to nav
+          real(8), dimension(4) :: q_bn ! quat. b frame
+          real(8), dimension(3) :: F_b, M_b ! force/mom. b frame
+          real(8), dimension(3) :: FA_w, MA_w ! aero force/mom. wind frame
+          real(8), dimension(3) :: FT_b, MT_b ! prop force/mom. body frame
+          real(8), dimension(4) :: dq_bn ! quaternion derivative
+          real(8), dimension(3) :: omega_bn_b ! ang. vel of b wrt n in b
+          real(8), dimension(3) :: wind_n ! wind in nav frame
+          real(8), dimension(3) :: wind_b ! wind in body frame
+          real(8), dimension(3) :: v_rel ! vel rel. to wind
+          real(8), dimension(3) :: v_b ! vel in body frame
+          real(8), dimension(3) :: dv_b ! deriv of vel in body frame
+          real(8), dimension(3) :: accel_b ! accel measurement, in body
+          real(8), dimension(3) :: B_n ! magnetic field in nav frame
+          real(8), dimension(3) :: B_b ! magnetic field in body frame
+          real(8), dimension(3) :: v_n ! vel in nav frame
+          real(8), dimension(3,3) :: J_b ! mom. of inertia in body frame
+          real(8), dimension(3,3) :: inv_J_b ! inverse
+          real(8), dimension(3) :: g_n ! gravity in nav frame
+          real(8) :: CL, CD, CC ! aero coefficients
+
+          ! updates gps, mag, gyro, accel, baro, pitot
+          integer, parameter :: i_gps=1, i_mag=2, i_gyro=3, &
+                i_accel=4, i_baro=5, i_pitot=6
+
+          ! measurement data
+          real(8) :: V_T, alpha, beta, phi, theta, psi, &
+                  t_gps, y_V_N, y_V_E, y_V_D, y_p_N, y_p_E, y_h, &
+                  t_mag, y_B_x, y_B_y, y_B_z, &
+                  t_gyro, y_P, y_Q, y_R, &
+                  t_accel, y_a_x, y_a_y, y_a_z, &
+                  t_baro, y_P_a, &
+                  t_pitot, y_P_d
+
+          ! extract state vector
+
+          ! velocity in body frame
+          v_b = x_vect(1:3)
+
+          ! attitude quaternion
+          q_bn = x_vect(4:7)
+
+          ! angular velocity in body frame
+          omega_bn_b = x_vect(8:10)
+
+          ! position
+          p_N = x_vect(11)
+          p_E = x_vect(12)
+          h = x_vect(13)
+
+          ! actuator positions
+          delta_l = x_vect(14)
+          delta_r = x_vect(15)
+          delta_t = x_vect(16)
+
+          ! gyro biases
+          gyro_bias_x = x_vect(17)
+          gyro_bias_y = x_vect(18)
+          gyro_bias_z = x_vect(19)
+
+          ! extract input vector
+          delta_l_cmd = u_vect(1)
+          delta_r_cmd = u_vect(2)
+          delta_t_cmd = u_vect(3)
+          wind_n = u_vect(4:6)
+
+          ! inertia matrices
+          J_b = reshape((/J_x,0d0,-J_xz,&
+                          0d0,J_y,0d0,&
+                          -J_xz,0d0,J_z/), shape(J_b))
+          gam = J_x*J_z - J_xz*J_xz
+          inv_J_b = reshape((/J_z,0d0,J_xz,&
+                              0d0,gam/J_y,0d0,&
+                              J_xz,0d0,J_x/), shape(inv_J_b))/gam
+
+          ! dcm
+          call quaternion_to_dcm(q_bn, C_bn)
+          C_nb = transpose(C_bn)
+
+          ! euler
+          call dcm_to_euler(C_bn, phi, theta, psi)
+
+          ! aerodynamics
+          wind_b = matmul(C_bn, wind_n)
+          ! prevent singularity in alpha/beta
+          if (abs(v_rel(1)) < 0.001d0) then
+                v_rel(1) = sign(0.001d0, v_rel(1))
+          end if
+          v_rel = v_b - wind_b
+          V_T = norm2(v_rel)
+          q_bar = 0.5d0*rho*V_T*V_T
+          alpha = atan2(v_rel(3), v_b(1))
+          beta = asin(v_rel(2)/V_T)
+
+          ! wind to body dcm
+          C_bw(1,1) = cos(alpha)*cos(beta)
+          C_bw(1,2) = -cos(alpha)*sin(beta)
+          C_bw(1,3) = -sin(alpha)
+          C_bw(2,1) = sin(beta)
+          C_bw(2,2) = cos(beta)
+          C_bw(2,3) = 0.0d0
+          C_bw(3,1) = sin(alpha)*cos(beta)
+          C_bw(3,2) = -sin(alpha)*sin(beta)
+          C_bw(3,3) = cos(alpha)
+
+          ! gravity
+          g_n(1:2) = 0.0d0
+          g_n(3) = g_D
+
+          ! aerodynamics
+          CL = CL_0 + CL_alpha*alpha
+          CD = CD_dmin + k_CD_CL*(CL - CL_dmin)**2
+          CC = CC_beta*beta
+          FA_w(1) = -CD*q_bar*s ! drag
+          FA_w(2) = -CC*q_bar*s ! side force
+          FA_w(3) = -CL*q_bar*s ! lift
+          ! mixing for polynomials
+          de = (delta_l + delta_r)/2
+          da = (delta_l - delta_r)/2
+          ! TODO is PQR in correct frame for poly fit?
+          P = omega_bn_b(1)
+          Q = omega_bn_b(2)
+          R = omega_bn_b(3)
+          MA_w(1) = Cl_p*P + Cl_beta*beta &
+                + Cl_da*da
+          MA_w(2) = Cm_0 + Cm_alpha*alpha + Cm_q*Q &
+                + Cm_de*de
+          MA_w(3) = Cn_beta*beta + Cn_r*R
+
+          ! propulsion
+          FT_b(1) = T_max*delta_t
+          FT_b(2:3) = 0.0d0
+          MT_b(1:3) = 0.0d0
+
+          ! force equations
+          F_b = matmul(C_bw, FA_w) + FT_b
+          dv_b = F_b/m  + matmul(C_bn, g_n) - cross(omega_bn_b, v_b)
+          dx(1:3) = dv_b
+
+          ! rotational kinematics equations
+          call quaternion_derivative(q_bn, omega_bn_b, dq_bn)
+          dx(4:7) = dq_bn
+
+          ! moment equations
+          M_b = matmul(C_bw, MA_w) + MT_b
+          dx(8:10) = matmul(inv_J_b, &
+                M_b - cross(omega_bn_b, matmul(J_b, omega_bn_b)))
+
+          ! navigation equations: dv_N, dv_E, dv_D
+          v_n = matmul(C_nb, v_b)
+          dx(11:12) = v_n(1:2)
+          dx(13) = -v_n(3)
+
+          ! actuators: limit cmd to simulate saturation
+          ! TODO this may not be the best way, but avoids
+          ! complicated issues with integration, also
+          ! the plant is first order so there is no
+          ! overshoot
+          if (abs(delta_l_cmd) > elvn_max) then
+                delta_l_cmd = sign(elvn_max, delta_l_cmd)
+          end if
+          if (abs(delta_r_cmd) > elvn_max) then
+                delta_r_cmd = sign(elvn_max, delta_r_cmd)
+          end if
+          if (delta_t_cmd > 1) then
+                delta_t_cmd = 1
+          else if (delta_t_cmd < 0) then
+                delta_t_cmd = 0
+          end if
+          dx(14) = (delta_l_cmd - delta_l)/delta_l_tau
+          dx(15) = (delta_r_cmd - delta_r)/delta_r_tau
+          dx(16) = (delta_t_cmd - delta_t)/delta_t_tau
+
+          ! gyro bias, the gyro dynamics are stationary, but
+          ! there is random noise
+          dx(17:19) = 0.0d0
+
+          ! should we do a measurement
+          if (measure) then
+
+          ! old measurement data for zero order hold
+          ! first 6 measurements are just extra state data
+          t_gps = y(7)
+          y_V_N = y(8)
+          y_V_E = y(9)
+          y_V_D = y(10)
+          y_p_N = y(11)
+          y_p_E = y(12)
+          y_h = y(13)
+          t_mag = y(14)
+          y_B_x = y(15)
+          y_B_y = y(16)
+          y_B_z = y(17)
+          t_gyro = y(18)
+          y_P = y(19)
+          y_Q = y(20)
+          y_R = y(21)
+          t_accel = y(22)
+          y_a_x = y(23)
+          y_a_y = y(24)
+          y_a_z = y(25)
+          t_baro = y(26)
+          y_P_a = y(27)
+          t_pitot = y(28)
+          y_P_d = y(29)
+
+          ! gps measurement
+          call zero_order_hold(t, gps_period, t_gps, &
+                y_update(i_gps))
+          if (y_update(i_gps)) then
+                t_gps = t
+                y_V_N = adc(v_n(1) + gps_V_std*randn_scalar(), &
+                        -gps_V_max, gps_V_max, gps_V_res)
+                y_V_E = adc(v_n(2) + gps_V_std*randn_scalar(), &
+                        -gps_V_max, gps_V_max, gps_V_res)
+                y_V_D = adc(v_n(3) + gps_V_std*randn_scalar(), &
+                        -gps_V_max, gps_V_max, gps_V_res)
+                y_p_N = adc(p_N + gps_p_std*randn_scalar(), &
+                        -gps_p_max, gps_p_max, gps_p_res)
+                y_p_E = adc(p_E + gps_p_std*randn_scalar(), &
+                        -gps_p_max, gps_p_max, gps_p_res)
+                y_h = adc(h + gps_h_std*randn_scalar(), &
+                        gps_h_min, gps_h_max, gps_h_res)
+          end if
+
+          ! baro measurement
+          call zero_order_hold(t, baro_period, t_baro, &
+                y_update(i_baro))
+          if (y_update(i_baro)) then
+                t_baro = t
+                y_P_a = adc(0.0d0 + baro_std*randn_scalar(), &
+                        baro_min, baro_max, baro_res)
+          end if
+
+          ! pitot measurement
+          call zero_order_hold(t, pitot_period, t_pitot, &
+                y_update(i_pitot))
+          if (y_update(i_pitot)) then
+                t_pitot = t
+                y_P_d = adc(0.0d0 + pitot_std*randn_scalar(), &
+                        pitot_min, pitot_max, pitot_res)
+          end if
+
+          ! gyro measurement
+          call zero_order_hold(t, gyro_period, t_gyro, &
+                y_update(i_gyro))
+          if (y_update(i_gyro)) then
+                t_gyro = t
+                y_P = adc(P + gyro_bias_x + gyro_std*randn_scalar(), &
+                          -gyro_max, gyro_max, gyro_res)
+                y_Q = adc(Q + gyro_bias_y + gyro_std*randn_scalar(), &
+                          -gyro_max, gyro_max, gyro_res)
+                y_R = adc(R + gyro_bias_z + gyro_std*randn_scalar(), &
+                          -gyro_max, gyro_max, gyro_res)
+          end if
+
+          ! accel measurement
+          call zero_order_hold(t, accel_period, t_accel, &
+                y_update(i_accel))
+          if (y_update(i_accel)) then
+                t_accel = t
+                accel_b = dv_b - matmul(C_bn, g_n)
+                y_a_x = adc(accel_b(1) + accel_std*randn_scalar(), &
+                        -accel_max, accel_max, accel_res)
+                y_a_y = adc(accel_b(2) + accel_std*randn_scalar(), &
+                        -accel_max, accel_max, accel_res)
+                y_a_z = adc(accel_b(3) + accel_std*randn_scalar(), &
+                        -accel_max, accel_max, accel_res)
+          end if
+
+          ! mag measurement
+          call zero_order_hold(t, mag_period, t_mag, &
+                y_update(i_mag))
+          if (y_update(i_mag)) then
+                t_mag = t
+                ! TODO add mag dec/incl. to field/ params
+                B_n = (/1, 0, 0/)
+                B_b = matmul(C_bn, B_n)
+                y_B_x = adc(B_b(1) + mag_std*randn_scalar(), &
+                        -mag_max, mag_max, mag_res)
+                y_B_y = adc(B_b(2) + mag_std*randn_scalar(), &
+                        -mag_max, mag_max, mag_res)
+                y_B_z = adc(B_b(3) + mag_std*randn_scalar(), &
+                        -mag_max, mag_max, mag_res)
+          end if
+          
+          y = (/V_T, alpha, beta, phi, theta, psi, &
+                t_gps, y_V_N, y_V_E, y_V_D, y_p_N, y_p_E, y_h, &
+                t_mag, y_B_x, y_B_y, y_B_z, &
+                t_gyro, y_P, y_Q, y_R, &
+                t_accel, y_a_x, y_a_y, y_a_z, &
+                t_baro, y_P_a, &
+                t_pitot, y_P_d /)
+
+          ! measure end if
+          end if 
+
+      end subroutine dynamics
+
+      pure function adc(u, min, max, resolution)
+          real(8), intent(in) :: u
+          real(8), intent(in) :: min, max, resolution
+          real(8) :: adc
+          real(8) :: y
+          integer :: n
+          if (u < min) then
+                y = min
+          else if (u > max) then
+                y = max
+          else
+                y = u
+          end if
+          n = int(y/resolution)
+          adc = n*resolution
+      end function adc
+
+      subroutine zero_order_hold(t, period, time_stamp, update)
+          real(8), intent(in) :: t
+          real(8), intent(in) :: period
+          real(8), intent(inout) :: time_stamp
+          logical, intent(out) :: update
+          update = .false.
+          if (t - time_stamp >= period) then
+                time_stamp = t
+                update = .true.
+          end if
+      end subroutine zero_order_hold
+
+
+      subroutine normalize_quaternions(x_vect)
+          real(8), dimension(19), intent(inout) :: x_vect
+          real(8), dimension(4) :: q
+          q(1:4) = x_vect(4:7)
+          x_vect(4:7) = q/norm2(q)
+      end subroutine normalize_quaternions
+
+      pure function cross(a, b)
+          real(8), dimension(3) :: cross
+          real(8), dimension(3), intent(in) :: a, b
+          cross(1) = a(2) * b(3) - a(3) * b(2)
+          cross(2) = a(3) * b(1) - a(1) * b(3)
+          cross(3) = a(1) * b(2) - a(2) * b(1)
+      end function cross
+    
+      subroutine dcm_to_euler(C_ba, phi, theta, psi)
+          real(8), dimension(3,3), intent(in) :: C_ba
+          real(8), intent(out) :: phi, theta, psi
+          phi = atan2(C_ba(2,3), C_ba(3,3))
+          theta = -asin(C_ba(1,3))
+          psi = atan2(C_ba(1,2), C_ba(1,1))
+      end subroutine dcm_to_euler
+
+      subroutine euler_to_dcm(phi, theta, psi, C_ba)
+          real(8), intent(in) :: phi, theta, psi
+          real(8), dimension(3,3), intent(out) :: C_ba
+          C_ba(1,1) = cos(theta)*cos(psi)
+          C_ba(1,2) = cos(theta)*sin(psi)
+          C_ba(1,3) = -sin(theta)
+          C_ba(2,1) = -cos(phi)*sin(psi) + sin(phi)*sin(theta)*cos(psi)
+          C_ba(2,2) = cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi)
+          C_ba(2,3) = sin(phi)*cos(theta)
+          C_ba(3,1) = sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi)
+          C_ba(3,2) = -sin(phi)*cos(psi) + cos(phi)*sin(theta)*sin(psi)
+          C_ba(3,3) = cos(phi)*cos(theta)
+      end subroutine euler_to_dcm
+
+      subroutine quaternion_to_dcm(q, C)
+          real(8), dimension(4), intent(in) :: q
+          real(8), dimension(3,3), intent(out) :: C
+          real(8) :: q0, q1, q2, q3, q0q0, q1q1, q2q2, q3q3
+          q0 = q(1)
+          q1 = q(2)
+          q2 = q(3)
+          q3 = q(4)
+          q0q0 = q0*q0
+          q1q1 = q1*q1
+          q2q2 = q2*q2
+          q3q3 = q3*q3
+          C(1,1) = q0q0 + q1q1 - q2q2 - q3q3
+          C(1,2) = 2*(q1*q2 + q0*q3)
+          C(1,3) = 2*(q1*q3 - q0*q2)
+          C(2,1) = 2*(q1*q2 - q0*q3)
+          C(2,2) = q0q0 - q1q1 + q2q2 - q3q3
+          C(2,3) = 2*(q2*q3 + q0*q1)
+          C(3,1) = 2*(q1*q3 + q0*q2)
+          C(3,2) = 2*(q2*q3 - q0*q1)
+          C(3,3) = q0q0 - q1q1 - q2q2 + q3q3
+      end subroutine quaternion_to_dcm
+
+      subroutine quaternion_derivative(q, omega, dq)
+          real(8), dimension(4), intent(in) :: q
+          real(8), dimension(3), intent(in) :: omega
+          real(8), dimension(4), intent(out) :: dq
+          dq(1) = -omega(1)*q(2) &
+                     -omega(2)*q(3) &
+                     -omega(3)*q(4)
+          dq(2) = omega(1)*q(1) &
+                     +omega(3)*q(3) &
+                     -omega(2)*q(4)
+          dq(3) = omega(2)*q(1) &
+                     -omega(3)*q(2) &
+                     -omega(1)*q(4)
+          dq(4) = omega(3)*q(1) &
+                     +omega(2)*q(2) &
+                     -omega(1)*q(3)
+          dq = 0.5*dq
+      end subroutine quaternion_derivative
+
+      subroutine euler_to_quaternion(phi, theta, psi, q_ba)
+          real(8), intent(in) :: phi, theta, psi
+          real(8), dimension(4), intent(out) :: q_ba
+          q_ba(1) = cos(phi/2)*cos(theta/2)*cos(psi/2) + &
+                  sin(phi/2)*sin(theta/2)*sin(psi/2)
+          q_ba(2) = sin(phi/2)*cos(theta/2)*cos(psi/2) - &
+                  cos(phi/2)*sin(theta/2)*sin(psi/2)
+          q_ba(3) = cos(phi/2)*sin(theta/2)*cos(psi/2) + &
+                  sin(phi/2)*cos(theta/2)*sin(psi/2)
+          q_ba(4) = cos(phi/2)*cos(theta/2)*sin(psi/2) - &
+                  sin(phi/2)*sin(theta/2)*cos(psi/2)
+      end subroutine euler_to_quaternion
+
+      function get_random_seed_size()
+          integer :: get_random_seed_size
+          call random_seed(size=get_random_seed_size)
+      end function get_random_seed_size
+
+      subroutine set_random_seed(seed_array)
+          integer, dimension(:), intent(in) :: seed_array
+          call random_seed(put=seed_array)
+      end subroutine set_random_seed
+
+      function get_random_seed()
+          integer, allocatable :: get_random_seed(:)
+          integer :: seed_size
+          seed_size = get_random_seed_size()
+          allocate(get_random_seed(seed_size))
+          call random_seed(get=get_random_seed)
+      end function get_random_seed
+
+      function randn_scalar()
+          ! std gnu rand converted to normal using Box-Muller transform
+          ! http://en.wikipedia.org/wiki/Box-Muller_transform
+          real(8) :: r1, r2, randn_scalar
+          call random_number(r1)
+          call random_number(r2)
+          randn_scalar = sqrt(r1)*sin(r2)
+      end function randn_scalar
+ 
+      subroutine randn(A)
+          ! std gnu rand converted to normal using Box-Muller transform
+          ! http://en.wikipedia.org/wiki/Box-Muller_transform
+          real(8), dimension(:,:), intent(inout) :: A
+          real(8), dimension(:,:), allocatable :: r1, r2
+          integer :: i, j, m, n, A_shape(2)
+          A_shape = shape(A)
+          m = A_shape(1)
+          n = A_shape(2)
+          allocate(r1(m,n))
+          allocate(r2(m,n))
+          call random_number(r1)
+          call random_number(r2)
+          do i=1,m
+                do j=1,n
+                        A(i,j) = sqrt(r1(i,j))*sin(r2(i,j))
+                end do
+          end do
+          deallocate(r1)
+          deallocate(r2)
+      end subroutine randn
+
+      end module camflyer

File nonlinear_model/camflyer_fortran.so

Binary file added.

File nonlinear_model/data.py

+import numpy as np
+
+
+class Data(object):
+    """
+    This is the simulation data structure.
+    """
+    x_names = 'V_X,V_Y,V_Z,a,b,c,d,P,Q,R,p_N,p_E,h,'\
+        'x_l,x_r,x_t,gyro_b_x,gyro_b_y,gyro_b_z'.split(',')
+    n_x = len(x_names)
+    x_titles = r'$V_X$,$V_Y$,$V_Z$,$a$,$b$,$c$,$d$,'\
+               r'$P$,$Q$,$R$,$p_N$,$p_E$,$h$,'\
+               r'$x_l$,$x_r$,$x_t$,gyro $b_x$,'\
+               r'gyro $b_y$, gyro $b_z$'.split(',')
+    x_dtype = np.dtype({
+        'names': x_names, 'titles': x_titles,
+        'formats': ['f8']*n_x})
+
+    u_names = 'delta_l,delta_r,delta_t,wind_x,wind_y,wind_z'.split(',')
+    u_titles = r'$delta_l$,$delta_r$,$delta_t$,'\
+               r'$wind_x$,$wind_y$,$wind_z$'.split(',')
+    n_u = len(u_names)
+    u_dtype = np.dtype({
+        'names': u_names, 'titles': u_titles,
+        'formats': ['f8']*n_u})
+
+    y_names = r'V_T,alpha,beta,phi,theta,psi,'\
+        r't_gps,V_N,V_E,V_D,p_N,p_E,h,'\
+        r't_mag,B_x,B_y,B_z,'\
+        r't_gyro,P,Q,R,'\
+        r't_accel,a_x,a_y,a_z,'\
+        r't_baro,P_a,'\
+        r't_pitot,P_d'.split(',')
+    n_y = len(y_names)
+    y_titles = r'$V_T$,$\alpha$,$\beta$,$\phi$,$\theta$,$\psi$,'\
+               r'$t_{GPS}$,$V_N$,$V_E$,$V_D$,$p_N$,$p_E$,$h$,'\
+               r'$t_{mag}$,$B_x$,$B_y$,$B_z$,'\
+               r'$t_{gyro}$,$P$,$Q$,$R$,'\
+               r'$t_{accel}$,$a_x$,$a_y$,$a_z$,'\
+               r'$t_{baro}$,$P_a$,'\
+               r'$t_{pitot}$,$P_d$'.split(',')
+    y_dtype = np.dtype({
+        'names': y_names, 'titles': y_titles,
+        'formats': ['f8']*n_y})
+
+    y_update_names = 'gps,mag,gyro,accel,baro,pitot'.split(',')
+    n_y_update = len(y_update_names)
+    y_update_dtype = np.dtype({
+        'names': y_update_names,
+        'formats': ['bool']*n_y_update})
+
+    def __init__(self, n_t):
+
+        self.n_t = n_t
+        self.t = np.zeros(self.n_t, dtype='f8')
+        self.x = np.zeros((self.n_t, self.n_x), dtype='f8')
+        self.u = np.zeros((self.n_t, self.n_u), dtype='f8')
+        self.y = np.zeros((self.n_t, self.n_y), dtype='f8')
+        self.y_update = np.zeros(
+            (self.n_t, self.n_y_update), dtype='bool')
+
+    def truncate(self, i):
+
+        self.t = self.t[:i]
+        self.x = self.x[:i, :]
+        self.u = self.u[:i, :]
+        self.y = self.y[:i, :]
+        self.y_update = self.y_update[:i, :]
+
+    def set(self, i, t, x, u, y, y_update):
+        self.t[i] = t
+        self.x[i, :] = x
+        self.u[i, :] = u
+        self.y[i, :] = y
+        self.y_update[i, :] = y_update

File nonlinear_model/wrapper.py

+import numpy as np
+import scipy.integrate
+
+"""
+This module serves to wrap the camflyer fortran simulation.
+"""
+
+from camflyer_fortran import camflyer as fortran
+
+
+class Aircraft(object):
+
+    def __init__(self, tf=10.0):
+        """
+        tf : final time (sec)
+
+        This  initialize the aircraft sim and allocates
+        all of the data structures.
+        """
+
+        process_noise_power = 0.01
+
+        params = {
+            # mass
+            'g_d': 9.8,
+            'm': 1,
+            'J_x': 0.1,
+            'J_y': 0.1,
+            'J_z': 0.1,
+            'J_xz': 0,
+            'rho': 1.225,
+            's': 1.0,
+            # aerodynamics
+            'CL_0': 0.1,
+            'CL_alpha': 0.1,
+            'CL_dmin': 0.2,
+            'CD_dmin': 0.01,
+            'k_CD_CL': 0.01,
+            'CC_beta': -0.1,
+            'Cl_beta': -0.1,
+            'Cl_p': -0.1,
+            'Cl_da': 0.1,
+            'Cm_0': 0.01,
+            'Cm_alpha': -0.1,
+            'Cm_q': -0.1,
+            'Cm_de': 0.1,
+            'Cn_beta': -0.01,
+            'Cn_r': -0.1,
+            'delta_l_tau': 0.1,
+            'delta_r_tau': 0.1,
+            'delta_t_tau': 0.1,
+            'elvn_max': 0.01,
+            # propulsion
+            't_max': 2,
+            # gps
+            'gps_period': 1.0/2,
+            'gps_V_std': 0.1,
+            'gps_V_res': 0.001,
+            'gps_V_max': 100,
+            'gps_p_std': 1.0,
+            'gps_p_res': 0.001,
+            'gps_p_max': 0.01,
+            'gps_h_std': 3.0,
+            'gps_h_res': 0.01,
+            'gps_h_min': -1e6,
+            'gps_h_max': 1e6,
+            # baro
+            'baro_period': 1.0/10,
+            'baro_std': 0.1,
+            'baro_res': 0.01,
+            'baro_min': -1e6,
+            'baro_max': 1e6,
+            # pitot
+            'pitot_period': 1.0/10,
+            'pitot_std': 0.1,
+            'pitot_res': 0.01,
+            'pitot_min': 0,
+            'pitot_max': 100,
+            # mag
+            'mag_period': 1.0/50,
+            'mag_std': 0.1,
+            'mag_res': 0.01,
+            'mag_max': 1e3,
+            # gyro
+            'gyro_period': 1.0/200,
+            'gyro_std': 0.01,
+            'gyro_res': 0.01,
+            'gyro_max': 1e3,
+            # accel
+            'accel_period': 1.0/200,
+            'accel_std': 0.01,
+            'accel_res': 0.01,
+            'accel_max': 1e3,
+        }
+        # f2py likes all lowercase since
+        # fortran is NOT case senstive
+        params = {key.lower(): params[key]
+                  for key in params.keys()}
+
+        # initial input
+        u0 = np.zeros(Data.n_u)
+        u0_p = u0.view(dtype=Data.u_dtype, type=np.recarray)
+        u0_p.delta_t = 0.5
+
+        # simulation time
+        # for noise to have sufficient bandwidth should run sim at
+        # 100*f_max, where f_max is the fastest closed loop pole
+        # frequency, for our system the actuators are 10 Hz and will
+        # likely be the fastest poles so we will simulate at
+        # 1000 Hz
+        dt = 1.0/1000
+        n_t = int((tf-dt)/dt)
+
+        f_dynamics = lambda t, x, u, y, measure, params: \
+            fortran.dynamics(
+                t, x, u_vect=u, y=y,
+                measure=measure, **params)
+
+        # intiialize simulation
+        sim = scipy.integrate.ode(f_dynamics)
+
+        x0 = np.zeros(Data.n_x)
+        x0_p = x0.view(dtype=Data.x_dtype, type=np.recarray)
+
+        x0_p.V_X[0] = 12  # velocity
+        x0_p.a = 1        # quaternion
+        x0_p.x_l = u0_p.delta_l  # actuator positions
+        x0_p.x_r = u0_p.delta_r
+        x0_p.x_t = u0_p.delta_t
+
+        x0[13:16] = u0[0:3]  # set actuators to trim position
+        sim.set_initial_value(x0)
+        fortran.normalize_quaternions(sim.y)
+        sim.set_integrator('dopri5')
+        data = Data(n_t)
+
+        # save data to class
+        self.sim = sim
+        self.data = data
+        self.process_noise_power = process_noise_power
+        self.u0 = u0
+        self.dt = dt
+        self.data = data
+        self.n_t = n_t
+        self.x0_p = x0_p
+        self.u0_p = u0_p
+        self.params = params
+        self.f_dynamics = f_dynamics
+
+    def simulate(self):
+
+        # get class data
+        params = self.params
+        u0 = self.u0
+        sim = self.sim
+        process_noise_power = self.process_noise_power
+        dt = self.dt
+        data = self.data
+        n_t = self.n_t
+        x0_p = self.x0_p
+        u0_p = self.u0_p
+
+        # intialize loop
+        u = u0
+        i = 0
+
+        y_i = np.zeros(Data.n_y, dtype='f8')
+        y_update_i = np.zeros(Data.n_y_update, dtype='bool')
+
+        # intialize random number generator
+        fortran.set_random_seed(np.random.randint(
+            0, 1e6, fortran.get_random_seed_size()))
+        # initialize process noise vector
+        w = np.zeros(len(sim.y), order='F')
+
+        # TODO add wind
+        wind_n = 0
+        wind_e = 0
+        wind_d = 0
+
+        # main simulation loop
+        while True:
+
+            # send parameters to fortran simulation function
+            # fort_params.update({'u_vect': u})
+            sim.set_f_params(u, np.zeros(29), False, params)
+
+            # integrate continuous dynamics
+            sim.integrate(sim.t + dt)
+            if not sim.successful():
+                print 'sim failed'
+                print sim.t, sim.y
+                break
+
+            # add process noise
+            # note we are using the fortran routine
+            # so that we have a consistent seed/
+            # noise generation algorithm, it is
+            # also faster than calling numpy.randn
+            # since it is using a preallocated matrix
+            # TODO is quaternion noise white?
+            fortran.randn(w)
+            w *= process_noise_power
+            sim.set_initial_value(
+                sim.y + process_noise_power*dt*w,
+                sim.t)
+
+            # normalize quaterions
+            fortran.normalize_quaternions(sim.y)
+
+            # take measurements
+            dx, y_update_i = self.f_dynamics(
+                t=sim.t, x=sim.y, u=u, y=y_i,
+                measure=True, params=params)
+
+            # yp_update_i = data.y_update.view(
+            #     dtype=Data.y_update_dtype, type=np.recarray)
+
+            # save data
+            data.set(i, sim.t, sim.y, u, y_i, y_update_i)
+
+            # exit if done
+            if (i+1 >= n_t):
+                break
+            else:
+                i += 1
+
+            # calculate new control
+            y = y_i.view(dtype=Data.y_dtype, type=np.recarray)
+            delta_a = -0.1*y.phi
+            delta_e = -0.1*y.theta
+            delta_t = -0.1*(x0_p.V_X - y.V_T) + u0_p.delta_t
+            # mixing
+            delta_l = delta_e + delta_a + u0_p.delta_l
+            delta_r = delta_e - delta_a + u0_p.delta_r
+
+            # set new input
+            u = [delta_l, delta_r, delta_t, wind_n, wind_e, wind_d]
+
+        data.truncate(i)
+
+
+class Data(object):
+    """
+    This is the simulation data structure.
+    """
+    x_names = 'V_X,V_Y,V_Z,a,b,c,d,P,Q,R,p_N,p_E,h,'\
+        'x_l,x_r,x_t,gyro_b_x,gyro_b_y,gyro_b_z'.split(',')
+    n_x = len(x_names)
+    x_titles = r'$V_X$,$V_Y$,$V_Z$,$a$,$b$,$c$,$d$,'\
+               r'$P$,$Q$,$R$,$p_N$,$p_E$,$h$,'\
+               r'$x_l$,$x_r$,$x_t$,gyro $b_x$,'\
+               r'gyro $b_y$, gyro $b_z$'.split(',')
+    x_dtype = np.dtype({
+        'names': x_names, 'titles': x_titles,
+        'formats': ['f8']*n_x})
+
+    u_names = 'delta_l,delta_r,delta_t,wind_x,wind_y,wind_z'.split(',')
+    u_titles = r'$delta_l$,$delta_r$,$delta_t$,'\
+               r'$wind_x$,$wind_y$,$wind_z$'.split(',')
+    n_u = len(u_names)
+    u_dtype = np.dtype({
+        'names': u_names, 'titles': u_titles,
+        'formats': ['f8']*n_u})
+
+    y_names = r'V_T,alpha,beta,phi,theta,psi,'\
+        r't_gps,V_N,V_E,V_D,p_N,p_E,h,'\
+        r't_mag,B_x,B_y,B_z,'\
+        r't_gyro,P,Q,R,'\
+        r't_accel,a_x,a_y,a_z,'\
+        r't_baro,P_a,'\
+        r't_pitot,P_d'.split(',')
+    n_y = len(y_names)
+    y_titles = r'$V_T$,$\alpha$,$\beta$,$\phi$,$\theta$,$\psi$,'\
+               r'$t_{GPS}$,$V_N$,$V_E$,$V_D$,$p_N$,$p_E$,$h$,'\
+               r'$t_{mag}$,$B_x$,$B_y$,$B_z$,'\
+               r'$t_{gyro}$,$P$,$Q$,$R$,'\
+               r'$t_{accel}$,$a_x$,$a_y$,$a_z$,'\
+               r'$t_{baro}$,$P_a$,'\
+               r'$t_{pitot}$,$P_d$'.split(',')
+    y_dtype = np.dtype({
+        'names': y_names, 'titles': y_titles,
+        'formats': ['f8']*n_y})
+
+    y_update_names = 'gps,mag,gyro,accel,baro,pitot'.split(',')
+    n_y_update = len(y_update_names)
+    y_update_dtype = np.dtype({
+        'names': y_update_names,
+        'formats': ['bool']*n_y_update})
+
+    def __init__(self, n_t):
+
+        self.n_t = n_t
+        self.t = np.zeros(self.n_t, dtype='f8')
+        self.x = np.zeros((self.n_t, self.n_x), dtype='f8')
+        self.u = np.zeros((self.n_t, self.n_u), dtype='f8')
+        self.y = np.zeros((self.n_t, self.n_y), dtype='f8')
+        self.y_update = np.zeros(
+            (self.n_t, self.n_y_update), dtype='bool')
+
+    def truncate(self, i):
+
+        self.t = self.t[:i]
+        self.x = self.x[:i, :]
+        self.u = self.u[:i, :]
+        self.y = self.y[:i, :]
+        self.y_update = self.y_update[:i, :]
+
+    def set(self, i, t, x, u, y, y_update):
+        self.t[i] = t
+        self.x[i, :] = x
+        self.u[i, :] = u
+        self.y[i, :] = y
+        self.y_update[i, :] = y_update

File nonlinear_model/zoh.py

+class ZeroOrderHold(object):
+    """
+    This class models a zero order hold block.
+    The update function takes the time and a callback function
+    to update the state of the block. The function is called
+    if the ZOH period has elapsed.
+    """
+    def __init__(self, period):
+        self.period = period
+        self.t = None
+        self.x = None
+        self.updated = False
+    def update(self, t, f):
+        if self.t is None or (t - self.t >= self.period):
+            self.t = t
+            self.x = f()
+            self.updated = True
+        return self.x
+import matplotlib.pyplot as plt
+import itertools
+import matplotlib
+import numpy as np
+
+from nonlinear_model import Aircraft, Data
+
+# do simulation
+ac = Aircraft()
+ac.simulate()
+data = ac.data
+
+# extract data, setup parameter access
+t = data.t
+x = data.x.view(dtype=Data.x_dtype, type=np.recarray)
+y = data.y.view(dtype=Data.y_dtype, type=np.recarray)
+u = data.u.view(dtype=Data.u_dtype, type=np.recarray)
+y_update = data.y_update.view(
+    dtype=Data.y_update_dtype, type=np.recarray)
+
+plt.close('all')
+plt.rcParams['lines.linewidth'] = 5
+plt.rcParams['lines.markersize'] = 10
+plt.rcParams['font.size'] = 15
+
+plt.figure()
+h = plt.plot(t, np.rad2deg(np.column_stack((x.P, x.Q, x.R))))
+plt.grid()
+plt.title('rotation rate')
+plt.xlabel('t (sec)')
+plt.ylabel('(deg/s)')
+plt.legend(['$P$', '$Q$', '$R$'], loc='best')
+plt.savefig('rotation_rate.pdf')
+
+plt.figure()
+h = plt.plot(t, np.column_stack((x.p_N, x.p_E, x.h)))
+plt.legend(['$p_N$', '$p_E$', 'h'], loc='best')
+plt.grid()
+plt.xlabel('t (sec)')
+plt.ylabel('(m)')
+plt.title('position')
+
+plt.figure()
+h = plt.plot(t, y.V_T)
+plt.legend(h, ['$V_T$'], loc='best')
+plt.grid()
+plt.xlabel('t (sec)')
+plt.ylabel('(m/s)')
+plt.title('$V_T$')
+
+plt.figure()
+h = plt.plot(t, np.rad2deg(np.column_stack((y.alpha, y.beta))))
+plt.legend(h, ['$\\alpha$', '$\\beta$'], loc='best')
+plt.xlabel('t (sec)')
+plt.ylabel('(deg)')
+plt.grid()
+plt.title('$\\alpha$, $\\beta$')
+
+plt.figure()
+h = plt.plot(t, np.rad2deg(np.column_stack((x.x_l, x.x_r))))
+hc = plt.plot(
+    t,
+    np.rad2deg(np.column_stack((u.delta_l, u.delta_r))), '--')
+plt.legend(
+    [h[0], h[1], hc[0], hc[1]],
+    ['$\\delta_l$', '$\\delta_r$',
+        '$ref_l$', '$ref_r$'], loc='best')
+plt.xlabel('t (sec)')
+plt.ylabel('(deg)')
+plt.grid()
+plt.title('elevons')
+
+plt.figure()
+h = plt.plot(t, x.x_t)
+hc = plt.plot(t, u.delta_t, '--')
+plt.legend([h[0], hc[0]], ['$\\delta_t$', '$ref_t$'], loc='best')
+plt.xlabel('t (sec)')
+plt.ylabel('(deg)')
+plt.grid()
+plt.title('propulsion')
+
+plt.figure()
+h = plt.plot(t, np.rad2deg(np.column_stack((
+    y.phi, y.theta, y.psi))))
+plt.legend(h, ['$\\phi$', '$\\theta$', '$\\psi$'], loc='best')
+plt.xlabel('t (sec)')
+plt.ylabel('(deg)')
+plt.grid()
+plt.title('euler angles')
+
+
+def plot_updates(updates, n_t):
+    fillstyle = itertools.cycle(matplotlib.lines.Line2D.fillStyles)
+    h = []
+    count = 0
+    for update in updates:
+        i_update = np.where(update[:n_t])[0]
+        h.append(plt.plot(
+            t[i_update],
+            count*np.ones(len(i_update)), '.',
+            markersize=20,
+            marker='o',
+            fillstyle=fillstyle.next())[0])
+        plt.gca().get_yaxis().set_visible(False)
+        count -= 1
+    plt.xlim(0, t[n_t])
+    plt.ylim(1, count)
+    return h
+
+plt.figure()
+h = plot_updates(
+    [y_update.gyro, y_update.mag, y_update.accel],
+    101)
+plt.legend(h, ['gyro', 'mag', 'accel'], ncol=3, loc='best')
+plt.xlabel('t (sec)')
+plt.grid()
+plt.title('fast updates')
+
+plt.figure()
+h = plot_updates(
+    [y_update.gps, y_update.pitot, y_update.baro],
+    1001)
+plt.legend(h, ['gps', 'pitot', 'baro'], ncol=3, loc='best')
+plt.xlabel('t (sec)')
+plt.grid()
+plt.title('slow updates')

File test/.gitignore

-.ipynb_checkpoints/
-.ropeproject/
-*.so
-*.pyc
-*.swp

File test/analysis/Untitled0.ipynb

-{
- "metadata": {
-  "name": "",
-  "signature": "sha256:b96eec4bd874c28b9971c4b7d87b0d49725b8c47e7d44947ae4ffe67bb361222"
- },
- "nbformat": 3,
- "nbformat_minor": 0,
- "worksheets": [
-  {
-   "cells": [
-    {
-     "cell_type": "code",
-     "collapsed": false,
-     "input": [
-      "import numpy as np\n",
-      "import scipy.integrate\n",
-      "import dynamics\n",
-      "import matplotlib.pyplot as plt\n",
-      "%matplotlib inline\n",
-      "%load_ext autoreload\n",
-      "%autoreload 2\n",
-      "reload(dynamics)"
-     ],
-     "language": "python",
-     "metadata": {},
-     "outputs": [
-      {
-       "output_type": "stream",
-       "stream": "stdout",
-       "text": [
-        "The autoreload extension is already loaded. To reload it, use:\n",
-        "  %reload_ext autoreload\n"
-       ]
-      },
-      {
-       "metadata": {},
-       "output_type": "pyout",
-       "prompt_number": 78,
-       "text": [
-        "<module 'dynamics' from 'dynamics.so'>"
-       ]
-      }
-     ],
-     "prompt_number": 78
-    },
-    {
-     "cell_type": "code",
-     "collapsed": false,
-     "input": [
-      "%%time\n",
-      "sim = scipy.integrate.ode(dynamics.dynamics)\n",
-      "sim.set_initial_value(0.001*np.random.randn(19))\n",
-      "sim.set_integrator('dopri5')\n",
-      "\n",
-      "g_d = 9.8\n",
-      "m = 1\n",
-      "J_x = 1\n",
-      "J_y = 1\n",
-      "J_z = 1\n",
-      "J_xz = 0\n",
-      "rho = 1.225\n",
-      "u = np.zeros(6)\n",
-      "dt = 1.0/200\n",
-      "tf = 10.0\n",
-      "n_t = int(tf/dt)\n",
-      "\n",
-      "y = np.zeros((n_t,19))\n",
-      "t = np.zeros(n_t)\n",
-      "i = 0\n",
-      "while True:\n",
-      "    u = np.zeros(6)\n",
-      "    sim.set_f_params(u, g_d, m, J_x, J_y, J_z, J_xz, rho)\n",
-      "    #dynamics.dynamics(sim.t, sim.y, u, g_d, m, J_x, J_y, J_z, J_xz, rho)\n",
-      "    sim.integrate(sim.t + dt)\n",
-      "    if not sim.successful():\n",
-      "        print 'sim failed'\n",
-      "        break\n",
-      "    y[i,:] = sim.y\n",
-      "    t[i] = sim.t\n",
-      "    if (i >= n_t-1):\n",
-      "        break\n",
-      "    else:\n",
-      "        i += 1"
-     ],
-     "language": "python",
-     "metadata": {},
-     "outputs": [
-      {
-       "output_type": "stream",
-       "stream": "stdout",
-       "text": [
-        "CPU times: user 1.05 s, sys: 2.91 ms, total: 1.05 s\n",
-        "Wall time: 1.05 s\n"
-       ]
-      }
-     ],
-     "prompt_number": 123
-    },
-    {
-     "cell_type": "code",
-     "collapsed": false,
-     "input": [
-      "plt.plot(t, y[:,:]);"
-     ],
-     "language": "python",
-     "metadata": {},
-     "outputs": [
-      {
-       "metadata": {},
-       "output_type": "display_data",
-       "png": "iVBORw0KGgoAAAANSUhEUgAAAYAAAAEACAYAAAC6d6FnAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nRK5CYII=\n",
-       "text": [
-        "<matplotlib.figure.Figure at 0xbe25250>"
-       ]
-      }
-     ],
-     "prompt_number": 124
-    },
-    {
-     "cell_type": "code",
-     "collapsed": false,
-     "input": [
-      "i"
-     ],
-     "language": "python",
-     "metadata": {},
-     "outputs": [
-      {
-       "metadata": {},
-       "output_type": "pyout",
-       "prompt_number": 100,
-       "text": [
-        "99999"
-       ]
-      }
-     ],
-     "prompt_number": 100
-    },
-    {
-     "cell_type": "code",
-     "collapsed": false,
-     "input": [],
-     "language": "python",
-     "metadata": {},
-     "outputs": []
-    }
-   ],
-   "metadata": {}
-  }
- ]
-}

File test/analysis/__init__.py

Empty file removed.

File test/analysis/aircraft.py

-import scipy.integrate
-import numpy as np
-from .autopilot import Autopilot
-from .zoh import ZeroOrderHold
-
-
-class Data(object):
-    """
-    This is a data structure that holds all of the simulation data.
-    """
-
-    def __init__(self, n_t, n_x, n_y, n_u):
-        """
-        Preallocates the simulation data given the anticipated sizes.
-        """
-        self.t = np.zeros(n_t)
-        self.x = np.zeros((n_t, n_x))
-        self.xh = np.zeros((n_t, n_x))
-        self.y = np.zeros((n_t, n_y))
-        self.u = np.zeros((n_t, n_u))
-
-    def set(self, i, t, x, xh, y, u):
-        """
-        Sets all of the data for the current iteration.
-        """
-        self.t[i] = t
-        self.x[i, :] = x
-        self.xh[i, :] = xh
-        self.y[i, :] = y
-        self.u[i, :] = u
-
-    def truncate(self, i):
-        """
-        Truncate data after ith index, useful for
-        deleting data after early termination.
-        """
-        self.t = self.t[:i]
-        self.x = self.x[:i, :]
-        self.xh = self.xh[:i, :]
-        self.y = self.y[:i, :]
-        self.u = self.u[:i, :]
-
-
-class Aircraft(object):
-    """
-    This class represents the physical system with
-    continuos aircraft dynamics and discrete measurements
-    with noise/ discrete actuators input.
-
-    It also contains the autopilot model representing the
-    discrete time control system.
-    """
-
-    def __init__(self, x0):
-        """
-        Initialize the simulation.
-        """
-
-        # initialize integrator
-        self.x0 = x0
-        self.sim = scipy.integrate.ode(self.dynamics)
-        self.sim.set_initial_value(self.x0)
-
-        # dimensions
-        self.n_x = 18
-        self.n_y = 18
-        self.n_u = 18
-
-        # noise power (sigma^2*dt)
-        self.process_noise_power = 0.00000001
-        self.gps_noise_power = 0.00001
-        self.accel_noise_power = 0.00001
-
-        # period
-        self.dt = 1.0/400.0  # noise sampling/ output rate
-        self.dt_autopilot = 1.0/200  # autopilot update rate
-        self.dt_gps = 1.0
-        self.dt_accel = 1.0/200  # actual 800 Hz,
-        # simulating at a slower rate, but matching noise power
-        # self.dt_gyro = 1.0/200 # actual 800 Hz,
-        # simulating at a slower rate, but matching noise power
-        # self.dt_mag = 1.0/100
-        # self.dt_pitot = 1.0/3
-        # self.dt_baro = 1.0/3
-
-        # subsystems
-        self.autopilot = Autopilot(x0)
-
-        # zero order holds
-        self.zoh_gps = ZeroOrderHold(period=self.dt_gps)
-        self.zoh_autopilot = ZeroOrderHold(period=self.dt_autopilot)
-
-    def dynamics(self, t, x, u):
-        """
-        This function models the continuous vehicle dynamics.
-
-        x (18): phi, theta, psi, P, Q, R, U, V, W, L, l, h,
-                left_elevon_pos, right_elevon_pos, rpm, gyro_bias(3)
-        u (6): left_elevon_cmd, right_elevon_cmd, throttle_cmd,
-               wind_x, wind_y, wind_z
-        """
-        # state
-        phi = x[0]
-        theta = x[1]
-        psi = x[2]
-        P = x[3]
-        Q = x[4]
-        R = x[5]
-        U = x[6]
-        V = x[7]
-        W = x[8]
-        L = x[9]
-        # l = x[10]
-        h = x[11]
-        left_elevon_pos = x[12]
-        right_elevon_pos = x[13]
-        thrust = x[14]
-        # gyro_bias_x = x[15]
-        # gyro_bias_y = x[16]
-        # gyro_bias_z = x[17]
-
-        # constants
-        g_D = 9.7803
-        m = 1
-        J_x = 1
-        J_y = 1
-        J_z = 1
-        J_xz = 0
-        gamma = J_x*J_z - J_xz**2
-        R_0 = 6378137
-
-        # forces
-        F_x = 0
-        F_y = 0
-        F_z = 0
-
-        # moments
-        M_x = 0
-        M_y = 0
-        M_z = 0
-
-        # input
-        left_elevon_cmd = u[0]
-        right_elevon_cmd = u[1]
-        thrust_cmd = u[2]
-        # wind_x = u[3]
-        # wind_y = u[4]
-        # wind_z = u[5]
-
-        # function aliases
-        sin = np.sin
-        cos = np.cos
-        tan = np.tan
-
-        # attitude dynamics
-        phi_dot = P + tan(theta)*(Q*sin(phi) + R*cos(phi))
-        theta_dot = Q*cos(phi) - R*sin(phi)
-        psi_dot = (Q*sin(phi) + R*cos(phi))/cos(theta)
-        P_dot = (J_xz*(J_x - J_y + J_z)*P*Q
-                 - (J_z*(J_z - J_z) + J_xz**2)*Q*R + J_z*M_x + J_xz*M_z)/gamma
-        Q_dot = ((J_z - J_x)*P*R - J_xz*(P**2 - R**2) + M_y)/J_y
-        R_dot = (((J_x - J_y)*J_x + J_xz**2)*P*Q
-                 - J_xz*(J_x - J_y + J_z)*Q*R + J_xz*M_x + J_x*M_z)/gamma
-
-        # translational dynamics
-        U_dot = R*V - Q*W - g_D*sin(theta) + F_x/m
-        V_dot = -R*U + P*W + g_D*sin(phi)*cos(theta) + F_y/m
-        W_dot = Q*U - P*V + g_D*cos(phi)*cos(theta) + F_z/m
-
-        # dcm from nav to body frame
-        C_bn = np.array([
-            [cos(theta)*cos(psi), cos(theta)*sin(psi), -sin(theta)],
-            [-cos(phi)*sin(psi) + sin(phi)*sin(theta)*cos(psi),
-             cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi),
-             sin(phi)*cos(theta)],
-            [sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi),
-             -sin(phi)*cos(psi) + cos(phi)*sin(theta)*sin(psi),
-             cos(phi)*cos(theta)],
-        ])
-
-        # dcm from body to nav frame
-        C_nb = C_bn.T
-
-        # find navigation velocities
-        vNav = C_nb.dot([U, V, W])
-        vN = vNav[0]
-        vE = vNav[1]
-        vD = vNav[2]
-
-        R = R_0 + h
-        L_dot = vN/R
-        l_dot = vE/(R*cos(L))
-        h_dot = -vD
-
-        # actuator dynamics
-        left_elevon_dot = 10*(left_elevon_cmd - left_elevon_pos)
-        right_elevon_dot = 10*(right_elevon_cmd - right_elevon_pos)
-        thrust_dot = 10*(thrust_cmd - thrust)
-
-        # gyro bias dynamics
-        gyro_bias_x_dot = 0
-        gyro_bias_y_dot = 0
-        gyro_bias_z_dot = 0
-
-        dx = [phi_dot, theta_dot, psi_dot,
-              P_dot, Q_dot, R_dot,
-              U_dot, V_dot, W_dot,
-              L_dot, l_dot, h_dot,
-              left_elevon_dot, right_elevon_dot, thrust_dot,
-              gyro_bias_x_dot, gyro_bias_y_dot, gyro_bias_z_dot]
-
-        return dx
-
-    def accel_measurement(self, t, x):
-        """
-        This function models the accelerometer measurement.
-
-        y (6): a_x, a_y, a_z
-        """
-
-        dx = self.dynamics(t, x)
-        y = dx[6:9]
-
-        v = np.random.randn(3)*np.sqrt(self.accel_noise_power/self.dt_accel)
-        y = x + v
-        return y
-
-    def gps_measurement(self, t, x):
-        """
-        This function models the global positioning system measurement.
-
-        y (6): v_N, v_E, v_D, L, l, h
-        """
-        v = np.random.randn(self.n_y)*np.sqrt(self.gps_noise_power/self.dt_gps)
-        y_gps = x + v
-        return y_gps
-
-    def simulate(self, tf=10.0):
-        """
-        This function simulates the system and returns a data structure.
-        """
-        n_t = np.floor(tf/self.dt) - 1
-        i = 0
-
-        # intialize data structure
-        data = Data(n_t, self.n_x, self.n_y, self.n_u)
-
-        while True:
-            x = self.sim.y
-            t = self.sim.t
-
-            # measure
-            y_gps = self.zoh_gps.update(t, lambda: self.gps_measurement(t, x))
-            y = y_gps
-
-            # autopilot
-            u = self.zoh_autopilot.update(
-                t,
-                lambda: self.autopilot.update(t, x, self.zoh_gps))
-            self.sim.set_f_params(u)
-
-            # save data
-            data.set(i, self.sim.t, x, self.autopilot.x, y, u)
-
-            # exit if done
-            if (i+1 >= n_t):
-                break
-
-            # integrate
-            self.sim.integrate(self.sim.t + self.dt)
-            if not self.sim.successful():
-                break
-
-            # add bandlimited white noise to state, note dt for sim
-            # should satisfy dt <= 1/(100*freq_max), where freq_max
-            # is the max freq of the dynamics in Hz this is a
-            # general rule to ensure white noise is simulated properly
-            # (has a large enough bandwidth)
-            w = np.random.randn(self.n_x)*np.sqrt(
-                self.process_noise_power/self.dt)
-            self.sim.set_initial_value(self.sim.y + w, self.sim.t)
-
-            # get ready for next loop
-            i += 1
-
-        # return all data, truncate to i^th data point
-        data.truncate(i)
-        return data

File test/analysis/autopilot.py

-from .zoh import ZeroOrderHold
-
-
-class Autopilot(object):
-    """
-    This roughly approximates the software running in
-    discrete time on the autopilot.
-    """
-
-    def __init__(self, x0):
-        """
-        Initialize the autopilot states etc.
-        """
-
-        self.dt_observer = 1.0/200
-        self.dt_controller = 1.0/50
-
-        self.zoh_controller = ZeroOrderHold(period=self.dt_controller)
-        self.zoh_observer = ZeroOrderHold(period=self.dt_observer)
-
-        self.x = x0
-
-    def observer(self, t, u, y, zoh_gps):
-        """
-        This function is the observer.
-        """
-        x0 = self.x
-        x = x0 + (x0 + u)*self.dt_observer
-        if zoh_gps.updated:
-            zoh_gps.updated = False
-            x = x + 1*(y - x)
-        self.x = x
-        return x
-
-    def controller(self, t, x):
-        """
-        This function is the controller.
-        """
-        u = -2*x
-        return u
-
-    def update(self, t, y, zoh_gps):
-        """
-        This function simulates the operating system scheduler
-        running on the autopilot.
-        """
-        # controller
-        u = self.zoh_controller.update(t, lambda: self.controller(t, self.x))
-        # observer
-        self.x = self.zoh_observer.update(
-            t, lambda: self.observer(t, u, y, zoh_gps))
-        return u

File test/analysis/build.sh

-#!/bin/bash
-f2py -c camflyer.f90 -m camflyer_fortran

File test/analysis/camflyer.f90

-      module camflyer
-
-      implicit none
-
-      contains
-
-      subroutine dynamics( &
-                t, &                            ! time
-                x_vect, &                       ! state
-                u_vect, &                       ! input
-                measure, &                      ! do a measurement?
-                g_D, m, J_x, J_y, J_z, J_xz, &  ! mass
-                rho, s, &                       ! aero
-                CL_0, CL_alpha, CL_dmin, &      ! lift
-                CD_dmin, k_CD_CL, &             ! drag
-                CC_beta, &                      ! side force
-                Cl_beta, Cl_p, Cl_da, &         ! rolling 
-                Cm_0, Cm_alpha, Cm_q, Cm_de, &  ! pitching
-                Cn_beta, Cn_r, &                ! yawing
-                delta_l_tau, delta_r_tau, &     ! left/right cntrl
-                elvn_max, &                     ! elevon max deflect.
-                T_max, delta_t_tau, &           ! propulsion
-                gps_period, baro_period, &      ! measurement timing
-                pitot_period, gyro_period, &
-                accel_period, mag_period, &
-                gps_V_res, gps_V_max, &  ! adc params
-                gps_p_res, gps_p_max, &
-                gps_h_res, gps_h_min, gps_h_max, &
-                baro_res, baro_min, baro_max, &
-                pitot_res, pitot_min, pitot_max, &
-                mag_res, mag_max, &
-                gyro_res, gyro_max, &
-                accel_res, accel_max, &
-                dx, &                           ! state deriv
-                y)                              ! measurement
-
-          ! x: u, v, w, a, b, c, d, P, Q, R, p_N, p_E, h
-          !    delta_l, delta_r, delta_t,
-          !    gyro_bias_x, gyro_bias_y, gyro_bias_z
-          ! u: delta_l_cmd, delta_r_cmd, delta_t,
-          !    wind_N, wind_E, wind_D
-
-          ! input
-          real(8), intent(in) :: t
-          real(8), dimension(19), intent(in) :: x_vect
-          real(8), dimension(6), intent(in) :: u_vect
-          logical, intent(in) :: measure
-          real(8), intent(in) :: g_D, m, J_x, J_y, J_z, J_xz
-          real(8), intent(in) :: rho, s, &
-                CL_0, CL_alpha, CL_dmin, & ! lift curve
-                CD_dmin, k_CD_CL, & ! drag polar
-                CC_beta, & ! beta
-                Cl_beta, Cl_p, Cl_da, & ! rolling
-                Cm_0, Cm_alpha, Cm_q, Cm_de, & ! pitching
-                Cn_beta, Cn_r, & ! yawing
-                delta_l_tau, delta_r_tau, & ! left/right cntrl
-                elvn_max, &                     ! elevon max deflect.
-                T_max, delta_t_tau, &  !propulsion 
-                gps_period, baro_period, &      ! measurement timing
-                pitot_period, gyro_period, &
-                accel_period, mag_period, &
-                gps_V_res, gps_V_max, &  ! adc params
-                gps_p_res, gps_p_max, &
-                gps_h_res, gps_h_min, gps_h_max, &
-                baro_res, baro_min, baro_max, &
-                pitot_res, pitot_min, pitot_max, &
-                mag_res, mag_max, &
-                gyro_res, gyro_max, &
-                accel_res, accel_max
-
-          ! output
-          real(8), dimension(19), intent(out) :: dx
-          real(8), intent(inout), dimension(29) :: y
-
-          ! local variables
-          real(8), dimension(3,3) :: C_bw, C_bn
-          real(8) :: p_N, p_E, h, &
-                delta_l, delta_r, delta_t, &
-                delta_l_cmd, delta_r_cmd, delta_t_cmd, &
-                gyro_bias_x, gyro_bias_y, gyro_bias_z, &
-                gam, q_bar, P, Q, R, de, da
-          real(8), dimension(3,3) :: C_nb ! dcm from bodty to nav
-          real(8), dimension(4) :: q_bn ! quat. b frame
-          real(8), dimension(3) :: F_b, M_b ! force/mom. b frame
-          real(8), dimension(3) :: FA_w, MA_w ! aero force/mom. wind frame
-          real(8), dimension(3) :: FT_b, MT_b ! prop force/mom. body frame
-          real(8), dimension(4) :: dq_bn ! quaternion derivative
-          real(8), dimension(3) :: omega_bn_b ! ang. vel of b wrt n in b
-          real(8), dimension(3) :: wind_n ! wind in nav frame
-          real(8), dimension(3) :: wind_b ! wind in body frame
-          real(8), dimension(3) :: v_rel ! vel rel. to wind
-          real(8), dimension(3) :: v_b ! vel in body frame
-          real(8), dimension(3) :: dv_b ! deriv of vel in body frame
-          real(8), dimension(3) :: v_n ! vel in nav frame
-          real(8), dimension(3,3) :: J_b ! mom. of inertia in body frame
-          real(8), dimension(3,3) :: inv_J_b ! inverse
-          real(8), dimension(3) :: g_n ! gravity in nav frame
-          real(8) :: CL, CD, CC ! aero coefficients
-
-          logical :: gps_update, baro_update, pitot_update, &
-                  gyro_update, accel_update, mag_update
-
-          ! measurement data
-          real(8) :: V_T, alpha, beta, phi, theta, psi, &
-                  t_gps, y_V_N, y_V_E, y_V_D, y_p_N, y_p_E, y_h, &
-                  t_mag, y_B_x, y_B_y, y_B_z, &
-                  t_gyro, y_P, y_Q, y_R, &
-                  t_accel, y_a_x, y_a_y, y_a_z, &
-                  t_baro, y_P_a, &
-                  t_pitot, y_P_d
-
-          ! extract state vector
<