API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 28th, 2020
86
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 7.78 KB | None | 0 0
raw download clone embed print report
  1. import numpy as np
  2.  
  3.  
  4. def C_b_v(angles):
  5.     """
  6.  
  7.    :param angles: Euler angles, np.ndarray, shape: (3,1)
  8.    :return: transition matrix from b-frame to v-frame, np.ndarray, shape: (3,3)
  9.    """
  10.     phi, theta, psi = angles.flatten()
  11.  
  12.     result = np.zeros(shape=(3, 3))
  13.     #first row
  14.     result[0, 0] = np.cos(psi) * np.cos(theta)
  15.     result[0, 1] = np.cos(psi) * np.sin(theta) * np.sin(phi) - np.sin(psi) * np.cos(phi)
  16.     result[0, 2] = np.cos(psi) * np.sin(theta) * np.cos(phi) + np.sin(psi) * np.sin(phi)
  17.  
  18.     # second row
  19.     result[1, 0] = np.sin(psi) * np.cos(theta)
  20.     result[1, 1] = np.sin(psi) * np.sin(theta) * np.sin(phi) + np.cos(psi) * np.cos(phi)
  21.     result[1, 2] = np.sin(psi) * np.sin(theta) * np.cos(phi) - np.cos(psi) * np.sin(phi)
  22.  
  23.     # third row
  24.     result[2, 0] = -np.sin(theta)
  25.     result[2, 1] = np.cos(theta) * np.sin(phi)
  26.     result[2, 2] = np.cos(theta) * np.cos(phi)
  27.  
  28.     return result
  29.  
  30.  
  31. def f_euler_update(x, u, w, delta_t):
  32.     """
  33.  
  34.    :param x: state vector, np.ndarray, shape: (15,1)
  35.    :param u: measurements vector, np.ndarray, shape: (6,1)
  36.    :param w: noise vector, np.ndarray, shape: (6,1)
  37.    :param delta_t: time step, scalar
  38.    :return: deltas of eulaer angles, np.ndarray, shape: (3,1)
  39.    """
  40.  
  41.     omega_x, omega_y, omega_z = u.flatten()[:3]
  42.     phi, theta, psi = x.flatten()[:3]
  43.  
  44.     result = np.zeros(shape=3)
  45.     result[0] = (omega_y * np.sin(phi) + omega_z * np.cos(phi)) * np.tan(theta) + omega_x
  46.     result[1] = omega_y * np.cos(phi) - omega_z * np.sin(phi)
  47.     result[2] = (omega_y * np.sin(phi) + omega_z * np.cos(phi)) * (1./np.cos(theta))
  48.  
  49.     return result.reshape(-1, 1) * delta_t
  50.  
  51. def omega_unbiased(omega, bias, noise):
  52.     return omega - bias - noise
  53.  
  54.  
  55. def acc_unbiased(acc, bias, noise):
  56.     return acc - bias - noise
  57.  
  58.  
  59. def f(x, u, w, delta_t, g_v = np.array([0, 0, 9.81])):
  60.     """
  61.  
  62.    :param x: state vector, np.ndarray, shape: (15,1)
  63.    :param u: measurements vector, np.ndarray, shape: (6,1)
  64.    :param w: noise vector, np.ndarray, shape: (6,1)
  65.    :param delta_t: time step, scalar
  66.    :param g_v: acceleration of gravity, np.ndarray: shape: (3,)
  67.    :return: state vector at the next time step, np.ndarray, shape: (15,1)
  68.    """
  69.  
  70.     result = np.zeros(shape=15)
  71.     angles = x.flatten()[:3]
  72.     pose_coordinates = x.flatten()[3:6] # x,y,z
  73.     velocity = x.flatten()[6:9] # v_x, v_y, v_z
  74.  
  75.     omega_imu = u.flatten()[:3] # measurements from gyroscope
  76.     acc_imu = u.flatten()[3:] # measurements from accelerometer
  77.  
  78.     noise_omega = w.flatten()[:3] # omega noise
  79.     noise_acc = w.flatten()[3:] # acceleration noise
  80.  
  81.     bias_omega = x.flatten()[9:12] # bias in gyroscope
  82.     bias_acc = x.flatten()[12:] # bias in accelerometer
  83.  
  84.     u_unbiased = np.hstack((omega_unbiased(omega=omega_imu, bias=bias_omega, noise=noise_omega),
  85.                             acc_unbiased(acc=acc_imu, bias=bias_acc, noise=noise_acc)))
  86.  
  87.     trans_matrix = C_b_v(angles)
  88.  
  89.     result[:3] = angles + f_euler_update(x=x, u=u_unbiased, w=w, delta_t=delta_t).flatten()
  90.     result[3:6] = pose_coordinates + velocity * delta_t + \
  91.                   0.5 * delta_t**2 * (trans_matrix @ u_unbiased[3:] + g_v)
  92.     result[6:9] = velocity + delta_t * (trans_matrix @ u_unbiased[3:] + g_v)
  93.     result[9:12] = bias_omega
  94.     result[12:15] = bias_acc
  95.  
  96.     return result.reshape(-1, 1)
  97.  
  98. def jac_f_euler_angles(x, u, delta_t):
  99.     """
  100.  
  101.    :param x: state vector, np.ndarray, shape: (15,1)
  102.    :param u: measurements vector, np.ndarray, shape: (6,1)
  103.    :param delta_t: time step, scalar
  104.    :return: the derivetive of f_euler_update function by angles.
  105.                np.ndarray, shape: (3, 3)
  106.    """
  107.  
  108.     phi, theta, psi = x.flatten()[:3]
  109.     omega_x, omega_y, omega_z = u.flatten()[:3]
  110.  
  111.     result = np.zeros(shape=(3,3))
  112.  
  113.     # first row
  114.     result[0, 0] = (omega_y * np.cos(phi) - omega_z * np.sin(phi)) * np.tan(theta)
  115.     result[0, 1] = (omega_y * np.sin(phi) + omega_z * np.cos(phi)) * (1./np.cos(theta))**2
  116.  
  117.     # second row
  118.     result[1, 0] =  -omega_y * np.sin(phi) - omega_z * np.cos(phi)
  119.  
  120.     # third row
  121.     result[2, 0] = (omega_y * np.cos(phi) - omega_z * np.sin(phi))*(1./np.cos(theta))
  122.     result[2, 1] = (omega_y * np.sin(phi) + omega_z * np.cos(phi))*(np.sin(theta)/(np.cos(theta)**2))
  123.  
  124.     return result * delta_t
  125.  
  126.  
  127. def jac_c_b_v_angles(angles, acc): # uff...
  128.     """
  129.  
  130.    :param angles: Euler angles, np.ndarray, shape: (3,1)
  131.    :param acc: accelerations, np.ndarray, shape: (3, 1)
  132.    :return: the derivetive of C_b_v @ acc function by angles.
  133.                np.ndarray, shape: (3, 3)
  134.    """
  135.  
  136.     phi, theta, psi = angles.flatten()
  137.     a_x, a_y, a_z = acc.flatten()
  138.  
  139.     result = np.zeros(shape=(3,3))
  140.  
  141.     # first row
  142.     result[0, 0] = a_y * (np.cos(psi) * np.sin(theta) * np.cos(phi) + np.sin(psi) * np.sin(phi)) + \
  143.                    a_z * (-np.cos(psi) * np.sin(theta) * np.sin(phi) + np.sin(psi) * np.cos(phi))
  144.     result[0, 1] = a_x * (-np.cos(psi) * np.sin(theta)) + \
  145.                    a_y * (np.cos(psi) * np.cos(theta) * np.sin(phi)) + \
  146.                    a_z * (np.cos(psi) * np.cos(theta) * np.cos(phi))
  147.     result[0, 2] = a_x * (-np.sin(psi) * np.cos(theta)) + \
  148.                    a_y * (-np.sin(psi) * np.sin(theta) * np.sin(phi) - np.cos(psi) * np.cos(phi)) + \
  149.                    a_z * (-np.sin(psi) * np.sin(theta) * np.cos(phi) + np.cos(psi) * np.sin(phi))
  150.  
  151.     # second row
  152.     result[1, 0] = a_y * (np.sin(psi) * np.sin(theta) * np.cos(phi) - np.cos(psi) * np.sin(phi)) + \
  153.                    a_z * (-np.sin(psi) * np.sin(theta) * np.sin(phi) - np.cos(psi) * np.cos(phi))
  154.     result[1, 1] = a_x * (-np.sin(psi) * np.sin(theta)) + \
  155.                    a_y * (np.sin(psi) * np.cos(theta) * np.sin(phi)) + \
  156.                    a_z * (np.sin(psi) * np.cos(theta) * np.cos(phi))
  157.     result[1, 2] = a_x * (np.cos(psi) * np.cos(theta)) + \
  158.                    a_y * (np.cos(psi) * np.sin(theta) * np.sin(phi) - np.sin(psi) * np.cos(phi)) + \
  159.                    a_z * (np.cos(psi) * np.sin(theta) * np.cos(phi) + np.sin(psi) * np.sin(phi))
  160.  
  161.     result[2, 0] = a_y * (np.cos(theta) * np.cos(psi)) + \
  162.                    a_z * (-np.cos(theta) * np.sin(phi))
  163.     result[2, 1] = a_x * (-np.cos(theta)) + \
  164.                    a_y * (-np.sin(theta) * np.sin(phi)) + \
  165.                    a_z * (-np.sin(theta) * np.cos(phi))
  166.     result[2, 2] = 0
  167.  
  168.     return result
  169.  
  170.  
  171. def jac_f_x(x, u, w, delta_t):
  172.     """
  173.  
  174.    :param x: state vector, np.ndarray, shape: (15,1)
  175.    :param u: measurements vector, np.ndarray, shape: (6,1)
  176.    :param delta_t: time step, scalar
  177.    :return: jacobian of transition function with respect to state
  178.                np.ndarray, shape: (15, 15)
  179.    """
  180.     angles = x.flatten()[:3]
  181.  
  182.     omega_imu = u.flatten()[:3]  # measurements from gyroscope
  183.     acc_imu = u.flatten()[3:]  # measurements from accelerometer
  184.  
  185.     noise_omega = w.flatten()[:3]  # omega noise
  186.     noise_acc = w.flatten()[3:]  # acceleration noise
  187.  
  188.     bias_omega = x.flatten()[9:12]  # bias in gyroscope
  189.     bias_acc = x.flatten()[12:]  # bias in accelerometer
  190.  
  191.     u_unbiased = np.hstack((omega_unbiased(omega=omega_imu, bias=bias_omega, noise=noise_omega),
  192.                             acc_unbiased(acc=acc_imu, bias=bias_acc, noise=noise_acc)))
  193.  
  194.     result = np.zeros(shape=(15, 15))
  195.  
  196.     result[:3, :3] = jac_f_euler_angles(x=x, u=u_unbiased, delta_t=delta_t)
  197.  
  198.     result[3:6, :3] = 0.5 * delta_t**2 * jac_c_b_v_angles(angles=angles, acc=u_unbiased.flatten()[3:])
  199.     result[3:6, 3:6] = np.identity(3)
  200.     result[3:6, 6:9] = delta_t * np.identity(3)
  201.  
  202.     result[6:9, :3] = delta_t * jac_c_b_v_angles(angles=angles, acc=u_unbiased.flatten()[3:])
  203.     result[6:9, 6:9] = np.identity(3)
  204.  
  205.     result[9:12, 9:12] = np.identity(3)
  206.     result[12:15, 12:15] = np.identity(3)
  207.    
  208.     return result
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!