Commits

James Goppert committed 0d6d95e

Working on model.

  • Participants
  • Parent commits f7854fc

Comments (0)

Files changed (14)

File data/figures/V_T.pdf

Binary file modified.

File data/figures/alpha_beta.pdf

Binary file modified.

File data/figures/elevons.pdf

Binary file modified.

File data/figures/euler_angles.pdf

Binary file modified.

File data/figures/fast_updates.pdf

Binary file modified.

File data/figures/position.pdf

Binary file modified.

File data/figures/propulsion.pdf

Binary file modified.

File data/figures/rotation_rate.pdf

Binary file modified.

File data/figures/slow_updates.pdf

Binary file modified.

File nonlinear_model/aircraft.py

         'accel_max': 1e3,
     }
 
-    def __init__(self, tf=100.0, seed=None, params=default_params):
+    def __init__(self, tf=100.0, params=default_params):
         """
         tf : final time (sec)
 
         n_t = int((tf-dt)/dt)
 
         # dynamics function
-        f_dynamics = lambda t, x, u, y, measure, params: \
+        f_dynamics = lambda t, x, u, y, measure, y_noise, params: \
             fortran.dynamics(
                 t=t, x_vect=x, u_vect=u, y=y,
                 measure=measure,
+                y_noise=y_noise,
                 **params)
 
         # setup integration
         # setup data structure
         data = Data(n_t)
 
-        # intialize random number generator
-        n_seed = fortran.get_random_seed_size()
-        if (seed is None):
-            fortran.set_random_seed(np.random.randint(
-                0, 1e6, n_seed))
-        else:
-            if (len(seed) != n_seed):
-                raise IOError(
-                    'length of seed must be {:d}'.format(n_seed))
-            fortran.set_random_seed(seed)
-
         # save data to class
         self.sim = sim
         self.data = data
                 u=u0.view(dtype='f8'),
                 y=np.zeros(Data.n_y),
                 measure=False,
+                y_noise=np.zeros(Data.n_y_noise),
                 params=self.params_for_fortran())
+
             dx = dx.view(dtype=Data.x_dtype, type=np.recarray)
 
             if get_trim:
 
             # send parameters to fortran simulation function
             # fort_params.update({'u_vect': u})
-            sim.set_f_params(u, y, False, params)
+            sim.set_f_params(u, y, False, np.zeros(Data.n_y_noise), params)
 
             # integrate continuous dynamics
             sim.integrate(sim.t + dt)
                 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)
+            w = process_noise_power*dt*np.random.randn(Data.n_x)
+            sim.set_initial_value(sim.y + w, sim.t)
 
             # normalize quaterions
             fortran.normalize_quaternions(sim.y)
             # take measurements
             dx, y_update = self.f_dynamics(
                 t=sim.t, x=sim.y, u=u, y=y,
-                measure=True, params=params)
+                measure=True,
+                y_noise=np.random.rand(Data.n_y_noise), params=params)
 
             # yp_update_i = data.y_update.view(
             #     dtype=Data.y_update_dtype, type=np.recarray)

File nonlinear_model/camflyer.f90

 
       contains
 
-      subroutine dynamics( &
+      pure subroutine dynamics( &
                 t, &                            ! time
                 x_vect, &                       ! state
                 u_vect, &                       ! input
                 measure, &                      ! do a measurement?
+                y_noise, &                      ! noise for measurements if taken
                 g_d, m, j_x, j_y, j_z, j_xz, &  ! mass
                 rho, s, b, c_bar, &             ! aero
                 cl_0, cl_alpha, &               ! lift
           real(8), dimension(19), intent(in) :: x_vect
           real(8), dimension(6), intent(in) :: u_vect
           logical, intent(in) :: measure
+          real(8), dimension(17), intent(in) :: y_noise ! noise for measurements if taken
           real(8), intent(in) :: &
                 g_d, m, j_x, j_y, j_z, j_xz, &  ! mass
                 rho, s, b, c_bar, &             ! aero
           beta = asin(v_rel(2)/v_t)
 
           ! wind to body dcm
-          c_bw = compute_c_bw(alpha, beta)
+          call compute_c_bw(alpha, beta, c_bw)
 
           ! gravity
           g_n(1:2) = 0.0d0
           m_b = matmul(c_bw, ma_w) + mt_b
           dx(8:10) = matmul(inv_j_b, &
                 m_b - cross(omega_nb_b, matmul(j_b, omega_nb_b)))
-          dx(8:10) = 0
 
           ! navigation equations: dp_n, dp_e, dh_d
           v_n = matmul(c_nb, v_b)
                 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(), &
+                y_v_n = adc(v_n(1) + gps_v_std*y_noise(1), &
                         -gps_v_max, gps_v_max, gps_v_res)
-                y_v_e = adc(v_n(2) + gps_v_std*randn_scalar(), &
+                y_v_e = adc(v_n(2) + gps_v_std*y_noise(2), &
                         -gps_v_max, gps_v_max, gps_v_res)
-                y_v_d = adc(v_n(3) + gps_v_std*randn_scalar(), &
+                y_v_d = adc(v_n(3) + gps_v_std*y_noise(3), &
                         -gps_v_max, gps_v_max, gps_v_res)
-                y_p_n = adc(p_n + gps_p_std*randn_scalar(), &
+                y_p_n = adc(p_n + gps_p_std*y_noise(4), &
                         -gps_p_max, gps_p_max, gps_p_res)
-                y_p_e = adc(p_e + gps_p_std*randn_scalar(), &
+                y_p_e = adc(p_e + gps_p_std*y_noise(5), &
                         -gps_p_max, gps_p_max, gps_p_res)
-                y_h = adc(h + gps_h_std*randn_scalar(), &
+                y_h = adc(h + gps_h_std*y_noise(6), &
                         gps_h_min, gps_h_max, gps_h_res)
           end if
 
                 y_update(i_baro))
           if (y_update(i_baro)) then
                 t_baro = t
-                y_p_a = adc(0.0d0 + baro_std*randn_scalar(), &
+                y_p_a = adc(0.0d0 + baro_std*y_noise(7), &
                         baro_min, baro_max, baro_res)
           end if
 
                 y_update(i_pitot))
           if (y_update(i_pitot)) then
                 t_pitot = t
-                y_p_d = adc(0.0d0 + pitot_std*randn_scalar(), &
+                y_p_d = adc(0.0d0 + pitot_std*y_noise(8), &
                         pitot_min, pitot_max, pitot_res)
           end if
 
                 y_update(i_gyro))
           if (y_update(i_gyro)) then
                 t_gyro = t
-                y_p = adc(p + gyro_bias_x + gyro_std*randn_scalar(), &
+                y_p = adc(p + gyro_bias_x + gyro_std*y_noise(9), &
                           -gyro_max, gyro_max, gyro_res)
-                y_q = adc(q + gyro_bias_y + gyro_std*randn_scalar(), &
+                y_q = adc(q + gyro_bias_y + gyro_std*y_noise(10), &
                           -gyro_max, gyro_max, gyro_res)
-                y_r = adc(r + gyro_bias_z + gyro_std*randn_scalar(), &
+                y_r = adc(r + gyro_bias_z + gyro_std*y_noise(11), &
                           -gyro_max, gyro_max, gyro_res)
           end if
 
           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(), &
+                y_a_x = adc(accel_b(1) + accel_std*y_noise(12), &
                         -accel_max, accel_max, accel_res)
-                y_a_y = adc(accel_b(2) + accel_std*randn_scalar(), &
+                y_a_y = adc(accel_b(2) + accel_std*y_noise(13), &
                         -accel_max, accel_max, accel_res)
-                y_a_z = adc(accel_b(3) + accel_std*randn_scalar(), &
+                y_a_z = adc(accel_b(3) + accel_std*y_noise(14), &
                         -accel_max, accel_max, accel_res)
           end if
 
                 ! 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(), &
+                y_b_x = adc(b_b(1) + mag_std*y_noise(15), &
                         -mag_max, mag_max, mag_res)
-                y_b_y = adc(b_b(2) + mag_std*randn_scalar(), &
+                y_b_y = adc(b_b(2) + mag_std*y_noise(16), &
                         -mag_max, mag_max, mag_res)
-                y_b_z = adc(b_b(3) + mag_std*randn_scalar(), &
+                y_b_z = adc(b_b(3) + mag_std*y_noise(17), &
                         -mag_max, mag_max, mag_res)
           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), intent(in) :: u, min, max, resolution
           real(8) :: adc
-          real(8) :: y
+          real(8) :: f
           integer :: n
           if (u < min) then
-                y = min
+                f = min
           else if (u > max) then
-                y = max
+                f = max
           else
-                y = u
+                f = u
           end if
-          n = int(y/resolution)
-          adc = n*resolution
+          adc = n*int(f/resolution)
       end function adc
 
-      subroutine zero_order_hold(t, period, time_stamp, update)
+      pure 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
           end if
       end subroutine zero_order_hold
 
-
-      subroutine normalize_quaternions(x_vect)
+      pure 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)
           cross(3) = a(1) * b(2) - a(2) * b(1)
       end function cross
 
-      pure function compute_c_bw(alpha, beta)
+      pure subroutine compute_c_bw(alpha, beta, c_bw)
           ! wind to body dcm
           ! lewis pg. 73 (tranpose of)
           ! u_b = c_bw u_w
-          real(8), dimension(3,3) :: compute_c_bw
           real(8), intent(in) :: alpha, beta
+          real(8), dimension(3,3), intent(out) :: c_bw
           real(8) :: calpha, cbeta, salpha, sbeta
           calpha = cos(alpha)
           cbeta = cos(beta)
           salpha = sin(alpha)
           sbeta = sin(beta)
-          compute_c_bw(1,1) = calpha*cbeta
-          compute_c_bw(1,2) = -calpha*sbeta
-          compute_c_bw(1,3) = -salpha
-          compute_c_bw(2,1) = sbeta
-          compute_c_bw(2,2) = cbeta
-          compute_c_bw(2,3) = 0.0d0
-          compute_c_bw(3,1) = salpha*cbeta
-          compute_c_bw(3,2) = -salpha*sbeta
-          compute_c_bw(3,3) = calpha
-      end function compute_c_bw
+          c_bw(1,1) = calpha*cbeta
+          c_bw(1,2) = -calpha*sbeta
+          c_bw(1,3) = -salpha
+          c_bw(2,1) = sbeta
+          c_bw(2,2) = cbeta
+          c_bw(2,3) = 0.0d0
+          c_bw(3,1) = salpha*cbeta
+          c_bw(3,2) = -salpha*sbeta
+          c_bw(3,3) = calpha
+      end subroutine compute_c_bw
     
-      subroutine dcm_to_euler(c_nb, phi, theta, psi)
+      pure subroutine dcm_to_euler(c_nb, phi, theta, psi)
           ! titterton pg. 46
           real(8), dimension(3,3), intent(in) :: c_nb
           real(8), intent(out) :: phi, theta, psi
           psi = atan2(c_nb(2,1), c_nb(1,1))
       end subroutine dcm_to_euler
 
-      subroutine euler_to_dcm(phi, theta, psi, c_nb)
+      pure subroutine quaternion_to_euler(q_nb, phi, theta, psi)
+          ! u_n = c_nb u_b
+          real(8), dimension(4), intent(in) :: q_nb
+          real(8), intent(out) :: phi, theta, psi
+          real(8), dimension(3,3) :: c_nb
+          call quaternion_to_dcm(q_nb, c_nb)
+          call dcm_to_euler(c_nb, phi, theta, psi)
+      end subroutine quaternion_to_euler
+
+      pure subroutine euler_to_dcm(phi, theta, psi, c_nb)
           ! u_n = c_nb u_b
           ! euler (body 321)
           ! u_n = psi(3) <- theta(2) <- phi(1) u_b
           c_nb(3,3) = cphi*ctheta
       end subroutine euler_to_dcm
 
-      subroutine quaternion_to_dcm(q_nb, c_nb)
+      pure subroutine quaternion_to_dcm(q_nb, c_nb)
           ! titterton pg. 45
           ! u_n = c_nb u_b
           real(8), dimension(4), intent(in) :: q_nb
           c_nb(3,3) = aa - bb - cc + dd
       end subroutine quaternion_to_dcm
 
-      subroutine quaternion_derivative(q_nb, omega_nb_b, dq_nb)
+      pure subroutine quaternion_derivative(q_nb, omega_nb_b, dq_nb)
           ! titterton pg. 44
           ! u_n = c_nb u_b
           real(8), dimension(4), intent(in) :: q_nb
           dq_nb(4) = -0.5d0*(c*wx - b*wy - a*wz)
       end subroutine quaternion_derivative
 
-      subroutine euler_to_quaternion(phi, theta, psi, q_nb)
+      pure subroutine euler_to_quaternion(phi, theta, psi, q_nb)
           ! u_n = c_nb u_b
           ! euler (body 321)
           ! u_n = psi(3) <- theta(2) <- phi(1) u_b
           ! + changed to - (typo in book) for q_nb(4)
       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 modified.

File nonlinear_model/data.py

         r't_baro,P_a,'\
         r't_pitot,P_d'.split(',')
     n_y = len(y_names)
+    n_y_noise = 17 # how many measurements that require noise
     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$,'\
 
     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, :]
+        self.t = self.t[:i+1]
+        self.x = self.x[:i+1, :]
+        self.u = self.u[:i+1, :]
+        self.y = self.y[:i+1, :]
+        self.y_update = self.y_update[:i+1, :]
 
     def set(self, i, t, x, u, y, y_update):
         self.t[i] = t
 figpath = os.path.join('data', 'figures')
 
 # do simulation
-ac = Aircraft(seed=[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12])
+ac = Aircraft()
 ac.simulate()
 data = ac.data