code: def _dcm_to_q(self, dcm):
          
          assert(dcm.shape == (3, 3))
          q = np.zeros(4)
  
          tr = np.trace(dcm)
          if tr > 0:
              s = np.sqrt(tr + 1.0)
              q[0] = s * 0.5
              s = 0.5 / s
              q[1] = (dcm[2][1] - dcm[1][2]) * s
              q[2] = (dcm[0][2] - dcm[2][0]) * s
              q[3] = (dcm[1][0] - dcm[0][1]) * s
          else:
              dcm_i = np.argmax(np.diag(dcm))
              dcm_j = (dcm_i + 1) % 3
              dcm_k = (dcm_i + 2) % 3
  
              s = np.sqrt((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
                           dcm[dcm_k][dcm_k]) + 1.0)
              q[dcm_i + 1] = s * 0.5
              s = 0.5 / s
              q[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s
              q[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s
              q[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s
  
          return q
docs: Create q from dcm
          Reference:
              - Shoemake, Quaternions,
              http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
  
          :param dcm: 3x3 dcm array
          returns: quaternion array