Commits

James Goppert committed d4c9401

Update for figures.

  • Participants
  • Parent commits b9b47b7

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

         # elevon max deflect
         'elvn_max': 0.25,
         # propulsion
-        'thrust_max': 1,
+        'thrust_max': -1,
         'delta_t_tau': 0.1,
         # gps
         'gps_period': 1.0/2,
         w = np.zeros(len(sim.y), order='F')
 
         # TODO add wind
-        #  wind_n = 0
-        #  wind_e = 0
-        #  wind_d = 0
+        # wind_n = 0
+        # wind_e = 0
+        # wind_d = 0
 
         # main simulation loop
         while True:
             fortran.normalize_quaternions(sim.y)
 
             # take measurements
-            dx, y_update_i = self.f_dynamics(
+            dx, y_update = self.f_dynamics(
                 t=sim.t, x=sim.y, u=u, y=y,
                 measure=True)
 
                 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.U - y.V_T) + u0.delta_t
-            # mixing
+            # yp = y.view(dtype=Data.y_dtype, type=np.recarray)
+            # delta_a = -0.1*yp.phi
+            # delta_e = -0.1*yp.theta
+            # V_T = np.sqrt(x0.U*x0.U + x0.V*x0.V + x0.W*x0.W)
+            # delta_t = -0.1*(V_T - yp.V_T) + u0.delta_t
+            # # mixing
             # delta_l = delta_e + delta_a + u0.delta_l
             # delta_r = delta_e - delta_a + u0.delta_r
-            # delta_l = u0.delta_l
-            # delta_r = u0.delta_r
-            # delta_t = u0.delta_t
-
-            # set new input
+            #
+            # # set new input
             # u = [delta_l, delta_r, delta_t, wind_n, wind_e, wind_d]
 
         data.truncate(i)

File nonlinear_model/camflyer.f90

 
           ! propulsion
           FT_b(1) = thrust_max*delta_t
+          ! write(*,*) 'FT_b(1)', FT_b(1)
           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)
+          F_b = matmul(C_bw, FA_w)  + FT_b
+          dv_b = F_b/m  + matmul(C_nb, g_n) - cross(omega_bn_b, v_b)
           dx(1:3) = dv_b
-          ! component version
-          ! dx(1) = R*V - Q*W - g_D*sin(theta)  + F_b(1)/m
-          ! dx(2) = -R*U + P*W + g_D*sin(phi)*cos(theta) + F_b(2)/m
-          ! dx(3) = Q*U - P*V + g_D*cos(phi)*cos(theta) + F_b(3)/m
 
           ! rotational kinematics equations
           call quaternion_derivative(q_bn, omega_bn_b, dq_bn)
           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)))
-          ! component version
-          ! dx(8) = (J_xz*(J_x - J_y + J_z)*P*Q &
-          !              - (J_z*(J_z - J_y) + J_xz*J_xz)*Q*R &
-          !              +  J_z*M_b(1) + J_xz*M_b(3))/gam
-          ! dx(9) = ((J_z - J_x)*P*R - J_xz*(P*P - R*R) + M_b(2))/gam
-          ! dx(10) = (((J_x - J_y)*J_x + J_x*J_x)*P*Q &
-          !          - J_xz*(J_x - J_y + J_z)*Q*R + J_xz*M_b(1) &
-          !          + J_x*M_b(3))/gam
 
           ! navigation equations: dv_N, dv_E, dv_D
           v_n = matmul(C_nb, v_b)
           C(3,1) = 2*(q1*q3 + q0*q2)
           C(3,2) = 2*(q2*q3 - q0*q1)
           C(3,3) = q0q0 - q1q1 - q2q2 + q3q3
+          C = transpose(C)
       end subroutine quaternion_to_dcm
 
       subroutine quaternion_derivative(q, omega, dq)