Exemplo n.º 1
0
def rotate_vector(vector, q):
    '''
    Rotates a vector, according to the given quaternions.
    Note that a single vector can be rotated into many orientations;
    or a row of vectors can all be rotated by a single quaternion.
    
    
    Parameters
    ----------
    vector : array, shape (3,) or (N,3)
        vector(s) to be rotated.
    q : array_like, shape ([3,4],) or (N,[3,4])
        quaternions or quaternion vectors.
    
    Returns
    -------
    rotated : array, shape (3,) or (N,3)
        rotated vector(s)
    
    Notes
    -----
    .. math::
        q \\circ \\left( {\\vec x \\cdot \\vec I} \\right) \\circ {q^{ - 1}} = \\left( {{\\bf{R}} \\cdot \\vec x} \\right) \\cdot \\vec I

    More info under 
    http://en.wikipedia.org/wiki/Quaternion
    
    Examples
    --------
    >>> mymat = eye(3)
    >>> myVector = r_[1,0,0]
    >>> quats = array([[0,0, sin(0.1)],[0, sin(0.2), 0]])
    >>> quat.rotate_vector(myVector, quats)
    array([[ 0.98006658,  0.19866933,  0.        ],
           [ 0.92106099,  0.        , -0.38941834]])

    >>> quat.rotate_vector(mymat, [0, 0, sin(0.1)])
    array([[ 0.98006658,  0.19866933,  0.        ],
           [-0.19866933,  0.98006658,  0.        ],
           [ 0.        ,  0.        ,  1.        ]])

    '''
    vector = np.atleast_2d(vector)
    qvector = np.hstack((np.zeros((vector.shape[0], 1)), vector))
    vRotated = quat.quatmult(q, quat.quatmult(qvector, quat.quatinv(q)))
    vRotated = vRotated[:, 1:]

    if min(vRotated.shape) == 1:
        vRotated = vRotated.ravel()

    return vRotated
Exemplo n.º 2
0
def rotate_vector(vector, q):
    '''
    Rotates a vector, according to the given quaternions.
    Note that a single vector can be rotated into many orientations;
    or a row of vectors can all be rotated by a single quaternion.
    
    
    Parameters
    ----------
    vector : array, shape (3,) or (N,3)
        vector(s) to be rotated.
    q : array_like, shape ([3,4],) or (N,[3,4])
        quaternions or quaternion vectors.
    
    Returns
    -------
    rotated : array, shape (3,) or (N,3)
        rotated vector(s)
    
    Notes
    -----
    .. math::
        q \\circ \\left( {\\vec x \\cdot \\vec I} \\right) \\circ {q^{ - 1}} = \\left( {{\\bf{R}} \\cdot \\vec x} \\right) \\cdot \\vec I

    More info under 
    http://en.wikipedia.org/wiki/Quaternion
    
    Examples
    --------
    >>> mymat = eye(3)
    >>> myVector = r_[1,0,0]
    >>> quats = array([[0,0, sin(0.1)],[0, sin(0.2), 0]])
    >>> quat.rotate_vector(myVector, quats)
    array([[ 0.98006658,  0.19866933,  0.        ],
           [ 0.92106099,  0.        , -0.38941834]])

    >>> quat.rotate_vector(mymat, [0, 0, sin(0.1)])
    array([[ 0.98006658,  0.19866933,  0.        ],
           [-0.19866933,  0.98006658,  0.        ],
           [ 0.        ,  0.        ,  1.        ]])

    '''
    vector = np.atleast_2d(vector)
    qvector = np.hstack((np.zeros((vector.shape[0],1)), vector))
    vRotated = quat.quatmult(q, quat.quatmult(qvector, quat.quatinv(q)))
    vRotated = vRotated[:,1:]

    if min(vRotated.shape)==1:
        vRotated = vRotated.ravel()

    return vRotated
Exemplo n.º 3
0
    def Update(self, Gyroscope, Accelerometer, Magnetometer):
        '''Calculate the best quaternion to the given measurement values.'''
        
        q = self.Quaternion; # short name local variable for readability

        # Reference direction of Earth's magnetic field
        h = vector.rotate_vector(Magnetometer, q)
        b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))

        # Gradient decent algorithm corrective step
        F = [2*(q[1]*q[3] - q[0]*q[2])   - Accelerometer[0],
             2*(q[0]*q[1] + q[2]*q[3])   - Accelerometer[1],
             2*(0.5 - q[1]**2 - q[2]**2) - Accelerometer[2],
             2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2])   - Magnetometer[0],
             2*b[1]*(q[1]*q[2] - q[0]*q[3])   + 2*b[3]*(q[0]*q[1] + q[2]*q[3])   - Magnetometer[1],
             2*b[1]*(q[0]*q[2] + q[1]*q[3])   + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - Magnetometer[2]]
        
        J = np.array([
            [-2*q[2],                 	2*q[3],                    -2*q[0],                         2*q[1]],
            [ 2*q[1],                 	2*q[0],                	    2*q[3],                         2*q[2]],
            [0,                        -4*q[1],                    -4*q[2],                         0],
            [-2*b[3]*q[2],              2*b[3]*q[3],               -4*b[1]*q[2]-2*b[3]*q[0],       -4*b[1]*q[3]+2*b[3]*q[1]],
            [-2*b[1]*q[3]+2*b[3]*q[1],	2*b[1]*q[2]+2*b[3]*q[0],    2*b[1]*q[1]+2*b[3]*q[3],       -2*b[1]*q[0]+2*b[3]*q[2]],
            [ 2*b[1]*q[2],              2*b[1]*q[3]-4*b[3]*q[1],    2*b[1]*q[0]-4*b[3]*q[2],        2*b[1]*q[1]]])
        
        step = J.T.dot(F)
        step = vector.normalize(step)	# normalise step magnitude

        # Compute rate of change of quaternion
        qDot = 0.5 * quat.quatmult(q, np.hstack([0, Gyroscope])) - self.Beta * step

        # Integrate to yield quaternion
        q = q + qDot * self.SamplePeriod
        self.Quaternion = vector.normalize(q).flatten()
Exemplo n.º 4
0
    def Update(self, Gyroscope, Accelerometer, Magnetometer):
        '''Calculate the best quaternion to the given measurement values.'''

        q = self.Quaternion
        # short name local variable for readability

        # Reference direction of Earth's magnetic field
        h = vector.rotate_vector(Magnetometer, q)
        b = np.hstack((0, np.sqrt(h[0]**2 + h[1]**2), 0, h[2]))

        # Estimated direction of gravity and magnetic field
        v = np.array([
            2 * (q[1] * q[3] - q[0] * q[2]), 2 * (q[0] * q[1] + q[2] * q[3]),
            q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2
        ])

        w = np.array([
            2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
            (q[1] * q[3] - q[0] * q[2]),
            2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
            (q[0] * q[1] + q[2] * q[3]), 2 * b[1] *
            (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] * (0.5 - q[1]**2 - q[2]**2)
        ])

        # Error is sum of cross product between estimated direction and measured direction of fields
        e = np.cross(Accelerometer, v) + np.cross(Magnetometer, w)

        if self.Ki > 0:
            self._eInt += e * self.SamplePeriod
        else:
            self._eInt = np.array([0, 0, 0], dtype=np.float)

        # Apply feedback terms
        Gyroscope += self.Kp * e + self.Ki * self._eInt

        # Compute rate of change of quaternion
        qDot = 0.5 * quat.quatmult(q, np.hstack([0, Gyroscope])).flatten()

        # Integrate to yield quaternion
        q += qDot * self.SamplePeriod

        self.Quaternion = vector.normalize(q)
Exemplo n.º 5
0
    def Update(self, Gyroscope, Accelerometer, Magnetometer):
        '''Calculate the best quaternion to the given measurement values.'''
        
        q = self.Quaternion; # short name local variable for readability

        # Reference direction of Earth's magnetic field
        h = vector.rotate_vector(Magnetometer, q)
        b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))

        # Estimated direction of gravity and magnetic field
        v = np.array([
             2*(q[1]*q[3] - q[0]*q[2]),
             2*(q[0]*q[1] + q[2]*q[3]),
             q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2])

        w = np.array([
             2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]),
             2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]),
             2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2)]) 

        # Error is sum of cross product between estimated direction and measured direction of fields
        e = np.cross(Accelerometer, v) + np.cross(Magnetometer, w) 
        
        if self.Ki > 0:
            self._eInt += e * self.SamplePeriod  
        else:
            self._eInt = np.array([0, 0, 0], dtype=np.float)
        
        # Apply feedback terms
        Gyroscope += self.Kp * e + self.Ki * self._eInt;            
        
        # Compute rate of change of quaternion
        qDot = 0.5 * quat.quatmult(q, np.hstack([0, Gyroscope])).flatten()

        # Integrate to yield quaternion
        q += qDot * self.SamplePeriod

        self.Quaternion = vector.normalize(q)
Exemplo n.º 6
0
def analyze3Dmarkers(MarkerPos, ReferencePos):
    '''
    Take recorded positions from 3 markers, and calculate center-of-mass (COM) and orientation
    Can be used e.g. for the analysis of Optotrac data.

    Parameters
    ----------
    MarkerPos : ndarray, shape (N,9)
        x/y/z coordinates of 3 markers

    ReferencePos : ndarray, shape (1,9)
        x/y/z coordinates of markers in the reference position

    Returns
    -------
    Position : ndarray, shape (N,3)
        x/y/z coordinates of COM, relative to the reference position
    Orientation : ndarray, shape (N,3)
        Orientation relative to reference orientation, expressed as quaternion

    Example
    -------
    >>> (PosOut, OrientOut) = analyze3Dmarkers(MarkerPos, ReferencePos)


    '''

    # Specify where the x-, y-, and z-position are located in the data
    cols = {'x': r_[(0, 3, 6)]}
    cols['y'] = cols['x'] + 1
    cols['z'] = cols['x'] + 2

    # Calculate the position
    cog = np.vstack((sum(MarkerPos[:, cols['x']],
                         axis=1), sum(MarkerPos[:, cols['y']], axis=1),
                     sum(MarkerPos[:, cols['z']], axis=1))).T / 3.

    cog_ref = np.vstack(
        (sum(ReferencePos[cols['x']]), sum(
            ReferencePos[cols['y']]), sum(ReferencePos[cols['z']]))).T / 3.

    position = cog - cog_ref

    # Calculate the orientation
    numPoints = len(MarkerPos)
    orientation = np.ones((numPoints, 3))

    refOrientation = vector.plane_orientation(ReferencePos[:3],
                                              ReferencePos[3:6],
                                              ReferencePos[6:])

    for ii in range(numPoints):
        '''The three points define a triangle. The first rotation is such
        that the orientation of the reference-triangle, defined as the
        direction perpendicular to the triangle, is rotated along the shortest
        path to the current orientation.
        In other words, this is a rotation outside the plane spanned by the three
        marker points.'''

        curOrientation = vector.plane_orientation(MarkerPos[ii, :3],
                                                  MarkerPos[ii, 3:6],
                                                  MarkerPos[ii, 6:])
        alpha = vector.angle(refOrientation, curOrientation)

        if alpha > 0:
            n1 = vector.normalize(np.cross(refOrientation, curOrientation))
            q1 = n1 * np.sin(alpha / 2)
        else:
            q1 = r_[0, 0, 0]

        # Now rotate the triangle into this orientation ...
        refPos_after_q1 = vector.rotate_vector(
            np.reshape(ReferencePos, (3, 3)), q1)
        '''Find which further rotation in the plane spanned by the three marker points
	is necessary to bring the data into the measured orientation.'''

        Marker_0 = MarkerPos[ii, :3]
        Marker_1 = MarkerPos[ii, 3:6]
        Vector10 = Marker_0 - Marker_1
        vector10_ref = refPos_after_q1[0] - refPos_after_q1[1]
        beta = vector.angle(Vector10, vector10_ref)

        q2 = curOrientation * np.sin(beta / 2)

        if np.cross(vector10_ref, Vector10).dot(curOrientation) <= 0:
            q2 = -q2
        orientation[ii, :] = quat.quatmult(q2, q1)

    return (position, orientation)
Exemplo n.º 7
0
def kalman_quat(rate, acc, omega, mag):
    '''
    Calclulate the orientation from IMU magnetometer data.

    Parameters
    ----------
    rate : float
    	   sample rate [Hz]	
    acc : (N,3) ndarray
    	  linear acceleration [m/sec^2]
    omega : (N,3) ndarray
    	  angular velocity [rad/sec]
    mag : (N,3) ndarray
    	  magnetic field orientation

    Returns
    -------
    qOut : (N,4) ndarray
    	   unit quaternion, describing the orientation relativ to the coordinate system spanned by the local magnetic field, and gravity

    Notes
    -----
    Based on "Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking" Yun, X. and Bachman, E.R., IEEE TRANSACTIONS ON ROBOTICS, VOL. 22, 1216-1227 (2006)

    '''

    numData = len(acc)

    # Set parameters for Kalman Filter
    tstep = 1. / rate
    tau = [0.5, 0.5, 0.5]  # from Yun, 2006

    # Initializations
    x_k = np.zeros(7)  # state vector
    z_k = np.zeros(7)  # measurement vector
    z_k_pre = np.zeros(7)
    P_k = np.matrix(np.eye(7))  # error covariance matrix P_k

    Phi_k = np.matrix(np.zeros(
        (7, 7)))  # discrete state transition matrix Phi_k
    for ii in range(3):
        Phi_k[ii, ii] = np.exp(-tstep / tau[ii])

    H_k = np.matrix(np.eye(7))  # Identity matrix

    Q_k = np.matrix(np.zeros((7, 7)))  # process noise matrix Q_k
    D = 0.0001 * r_[0.4, 0.4, 0.4]  # [rad^2/sec^2]; from Yun, 2006
    # check 0.0001 in Yun
    for ii in range(3):
        Q_k[ii,
            ii] = D[ii] / (2 * tau[ii]) * (1 - np.exp(-2 * tstep / tau[ii]))

    # Evaluate measurement noise covariance matrix R_k
    R_k = np.matrix(np.zeros((7, 7)))
    r_angvel = 0.01
    # [rad**2/sec**2]; from Yun, 2006
    r_quats = 0.0001
    # from Yun, 2006
    for ii in range(7):
        if ii < 3:
            R_k[ii, ii] = r_angvel
        else:
            R_k[ii, ii] = r_quats

    # Calculation of orientation for every time step
    qOut = np.zeros((numData, 4))

    for ii in range(numData):
        accelVec = acc[ii, :]
        magVec = mag[ii, :]
        angvelVec = omega[ii, :]
        z_k_pre = z_k.copy(
        )  # watch out: by default, Python passes the reference!!

        # Evaluate quaternion based on acceleration and magnetic field data
        accelVec_n = vector.normalize(accelVec)
        magVec_hor = magVec - accelVec_n * accelVec_n.dot(magVec)
        magVec_n = vector.normalize(magVec_hor)
        basisVectors = np.vstack((magVec_n, np.cross(accelVec_n,
                                                     magVec_n), accelVec_n)).T
        quatRef = quat.quatinv(quat.rotmat2quat(basisVectors)).flatten()

        # Update measurement vector z_k
        z_k[:3] = angvelVec
        z_k[3:] = quatRef

        # Calculate Kalman Gain
        # K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
        # Check: why is H_k used in the original formulas?
        K_k = P_k * np.linalg.inv(P_k + R_k)

        # Update state vector x_k
        x_k += np.array(K_k.dot(z_k - z_k_pre)).ravel()

        # Evaluate discrete state transition matrix Phi_k
        Phi_k[3, :] = r_[-x_k[4] * tstep / 2, -x_k[5] * tstep / 2,
                         -x_k[6] * tstep / 2, 1, -x_k[0] * tstep / 2,
                         -x_k[1] * tstep / 2, -x_k[2] * tstep / 2]
        Phi_k[4, :] = r_[x_k[3] * tstep / 2, -x_k[6] * tstep / 2,
                         x_k[5] * tstep / 2, x_k[0] * tstep / 2, 1,
                         x_k[2] * tstep / 2, -x_k[1] * tstep / 2]
        Phi_k[5, :] = r_[x_k[6] * tstep / 2, x_k[3] * tstep / 2,
                         -x_k[4] * tstep / 2, x_k[1] * tstep / 2,
                         -x_k[2] * tstep / 2, 1, x_k[0] * tstep / 2]
        Phi_k[6, :] = r_[-x_k[5] * tstep / 2, x_k[4] * tstep / 2,
                         x_k[3] * tstep / 2, x_k[2] * tstep / 2,
                         x_k[1] * tstep / 2, -x_k[0] * tstep / 2, 1]

        # Update error covariance matrix
        #P_k = (eye(7)-K_k*H_k)*P_k
        # Check: why is H_k used in the original formulas?
        P_k = (H_k - K_k) * P_k

        # Projection of state quaternions
        x_k[3:] += quat.quatmult(0.5 * x_k[3:], r_[0, x_k[:3]]).flatten()
        x_k[3:] = vector.normalize(x_k[3:])
        x_k[:3] = np.zeros(3)
        x_k[:3] = tstep * (-x_k[:3] + z_k[:3])

        qOut[ii, :] = x_k[3:]

        # Projection of error covariance matrix
        P_k = Phi_k * P_k * Phi_k.T + Q_k

    # Make the first position the reference position
    qOut = quat.quatmult(qOut, quat.quatinv(qOut[0]))

    return qOut
Exemplo n.º 8
0
def calc_QPos(R_initialOrientation, omega, initialPosition, accMeasured, rate):
    ''' Reconstruct position and orientation, from angular velocity and linear acceleration.
    Assumes a start in a stationary position. No compensation for drift.

    Parameters
    ----------
    omega : ndarray(N,3)
        Angular velocity, in [rad/s]
    accMeasured : ndarray(N,3)
        Linear acceleration, in [m/s^2]
    initialPosition : ndarray(3,)
        initial Position, in [m]
    R_initialOrientation: ndarray(3,3)
        Rotation matrix describing the initial orientation of the sensor,
        except a mis-orienation with respect to gravity
    rate : float
        sampling rate, in [Hz]

    Returns
    -------
    q : ndarray(N,3)
        Orientation, expressed as a quaternion vector
    pos : ndarray(N,3)
        Position in space [m]

    Example
    -------
    >>> q1, pos1 = calc_QPos(R_initialOrientation, omega, initialPosition, acc, rate)

    '''

    # Transform recordings to angVel/acceleration in space --------------

    # Orientation of \vec{g} with the sensor in the "R_initialOrientation"
    g = 9.81
    g0 = np.linalg.inv(R_initialOrientation).dot(r_[0, 0, g])

    # for the remaining deviation, assume the shortest rotation to there
    q0 = vector.qrotate(accMeasured[0], g0)
    R0 = quat.quat2rotmat(q0)

    # combine the two, to form a reference orientation. Note that the sequence
    # is very important!
    R_ref = R_initialOrientation.dot(R0)
    q_ref = quat.rotmat2quat(R_ref)

    # Calculate orientation q by "integrating" omega -----------------
    q = quat.vel2quat(omega, q_ref, rate, 'bf')

    # Acceleration, velocity, and position ----------------------------
    # From q and the measured acceleration, get the \frac{d^2x}{dt^2}
    g_v = r_[0, 0, g]
    accReSensor = accMeasured - vector.rotate_vector(g_v, quat.quatinv(q))
    accReSpace = vector.rotate_vector(accReSensor, q)

    # Make the first position the reference position
    q = quat.quatmult(q, quat.quatinv(q[0]))

    # compensate for drift
    #drift = np.mean(accReSpace, 0)
    #accReSpace -= drift*0.7

    # Position and Velocity through integration, assuming 0-velocity at t=0
    vel = np.nan * np.ones_like(accReSpace)
    pos = np.nan * np.ones_like(accReSpace)

    for ii in range(accReSpace.shape[1]):
        vel[:, ii] = cumtrapz(accReSpace[:, ii], dx=1. / rate, initial=0)
        pos[:, ii] = cumtrapz(vel[:, ii],
                              dx=1. / rate,
                              initial=initialPosition[ii])

    return (q, pos)
Exemplo n.º 9
0
def analyze3Dmarkers(MarkerPos, ReferencePos):
    '''
    Take recorded positions from 3 markers, and calculate center-of-mass (COM) and orientation
    Can be used e.g. for the analysis of Optotrac data.

    Parameters
    ----------
    MarkerPos : ndarray, shape (N,9)
        x/y/z coordinates of 3 markers

    ReferencePos : ndarray, shape (1,9)
        x/y/z coordinates of markers in the reference position

    Returns
    -------
    Position : ndarray, shape (N,3)
        x/y/z coordinates of COM, relative to the reference position
    Orientation : ndarray, shape (N,3)
        Orientation relative to reference orientation, expressed as quaternion

    Example
    -------
    >>> (PosOut, OrientOut) = analyze3Dmarkers(MarkerPos, ReferencePos)


    '''

    # Specify where the x-, y-, and z-position are located in the data
    cols = {'x' : r_[(0,3,6)]} 
    cols['y'] = cols['x'] + 1
    cols['z'] = cols['x'] + 2    

    # Calculate the position
    cog = np.vstack(( sum(MarkerPos[:,cols['x']], axis=1),
                      sum(MarkerPos[:,cols['y']], axis=1),
                      sum(MarkerPos[:,cols['z']], axis=1) )).T/3.

    cog_ref = np.vstack(( sum(ReferencePos[cols['x']]),
                          sum(ReferencePos[cols['y']]),
                          sum(ReferencePos[cols['z']]) )).T/3.                      

    position = cog - cog_ref    

    # Calculate the orientation    
    numPoints = len(MarkerPos)
    orientation = np.ones((numPoints,3))

    refOrientation = vector.plane_orientation(ReferencePos[:3], ReferencePos[3:6], ReferencePos[6:])

    for ii in range(numPoints):
        '''The three points define a triangle. The first rotation is such
        that the orientation of the reference-triangle, defined as the
        direction perpendicular to the triangle, is rotated along the shortest
        path to the current orientation.
        In other words, this is a rotation outside the plane spanned by the three
        marker points.'''

        curOrientation = vector.plane_orientation( MarkerPos[ii,:3], MarkerPos[ii,3:6], MarkerPos[ii,6:])
        alpha = vector.angle(refOrientation, curOrientation)        

        if alpha > 0:
            n1 = vector.normalize( np.cross(refOrientation, curOrientation) )
            q1 = n1 * np.sin(alpha/2)
        else:
            q1 = r_[0,0,0]

        # Now rotate the triangle into this orientation ...
        refPos_after_q1 = vector.rotate_vector(np.reshape(ReferencePos,(3,3)), q1)

        '''Find which further rotation in the plane spanned by the three marker points
	is necessary to bring the data into the measured orientation.'''

        Marker_0 = MarkerPos[ii,:3]
        Marker_1 = MarkerPos[ii,3:6]
        Vector10 = Marker_0 - Marker_1
        vector10_ref = refPos_after_q1[0]-refPos_after_q1[1]
        beta = vector.angle(Vector10, vector10_ref)

        q2 = curOrientation * np.sin(beta/2)

        if np.cross(vector10_ref,Vector10).dot(curOrientation)<=0:
            q2 = -q2
        orientation[ii,:] = quat.quatmult(q2, q1)

    return (position, orientation)
Exemplo n.º 10
0
def kalman_quat(rate, acc, omega, mag):
    '''
    Calclulate the orientation from IMU magnetometer data.

    Parameters
    ----------
    rate : float
    	   sample rate [Hz]	
    acc : (N,3) ndarray
    	  linear acceleration [m/sec^2]
    omega : (N,3) ndarray
    	  angular velocity [rad/sec]
    mag : (N,3) ndarray
    	  magnetic field orientation

    Returns
    -------
    qOut : (N,4) ndarray
    	   unit quaternion, describing the orientation relativ to the coordinate system spanned by the local magnetic field, and gravity

    Notes
    -----
    Based on "Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking" Yun, X. and Bachman, E.R., IEEE TRANSACTIONS ON ROBOTICS, VOL. 22, 1216-1227 (2006)

    '''

    numData = len(acc)

    # Set parameters for Kalman Filter
    tstep = 1/rate
    tau = [0.5, 0.5, 0.5]	# from Yun, 2006

    # Initializations 
    x_k = np.zeros(7)	# state vector
    z_k = np.zeros(7)   # measurement vector
    z_k_pre = np.zeros(7)
    P_k = np.matrix( np.eye(7) )		 # error covariance matrix P_k

    Phi_k = np.matrix( np.zeros((7,7)) ) # discrete state transition matrix Phi_k
    for ii in range(3):
        Phi_k[ii,ii] = np.exp(-tstep/tau[ii])

    H_k = np.matrix( np.eye(7) )		# Identity matrix

    Q_k = np.matrix( np.zeros((7,7)) )	# process noise matrix Q_k
    D = 0.0001*r_[0.4, 0.4, 0.4]		# [rad^2/sec^2]; from Yun, 2006
                                                                            # check 0.0001 in Yun
    for ii in range(3):
        Q_k[ii,ii] =  D[ii]/(2*tau[ii])  * ( 1-np.exp(-2*tstep/tau[ii]) )

    # Evaluate measurement noise covariance matrix R_k
    R_k = np.matrix( np.zeros((7,7)) )
    r_angvel = 0.01;      # [rad**2/sec**2]; from Yun, 2006
    r_quats = 0.0001;     # from Yun, 2006
    for ii in range(7):
        if ii<3:
            R_k[ii,ii] = r_angvel
        else:
            R_k[ii,ii] = r_quats

    # Calculation of orientation for every time step
    qOut = np.zeros( (numData,4) )

    for ii in range(numData):
        accelVec  = acc[ii,:]
        magVec    = mag[ii,:]
        angvelVec = omega[ii,:]
        z_k_pre = z_k.copy()	# watch out: by default, Python passes the reference!!

        # Evaluate quaternion based on acceleration and magnetic field data
        accelVec_n = vector.normalize(accelVec)
        magVec_hor = magVec - accelVec_n * accelVec_n.dot(magVec)
        magVec_n   = vector.normalize(magVec_hor)
        basisVectors = np.vstack( (magVec_n, np.cross(accelVec_n, magVec_n), accelVec_n) ).T
        quatRef = quat.quatinv(quat.rotmat2quat(basisVectors)).flatten()

        # Update measurement vector z_k
        z_k[:3] = angvelVec
        z_k[3:] = quatRef

        # Calculate Kalman Gain
        # K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
        # Check: why is H_k used in the original formulas?
        K_k = P_k * np.linalg.inv(P_k + R_k)

        # Update state vector x_k
        x_k += np.array( K_k.dot(z_k-z_k_pre) ).ravel()

        # Evaluate discrete state transition matrix Phi_k
        Phi_k[3,:] = r_[-x_k[4]*tstep/2, -x_k[5]*tstep/2, -x_k[6]*tstep/2,              1, -x_k[0]*tstep/2, -x_k[1]*tstep/2, -x_k[2]*tstep/2]
        Phi_k[4,:] = r_[ x_k[3]*tstep/2, -x_k[6]*tstep/2,  x_k[5]*tstep/2, x_k[0]*tstep/2,               1,  x_k[2]*tstep/2, -x_k[1]*tstep/2]
        Phi_k[5,:] = r_[ x_k[6]*tstep/2,  x_k[3]*tstep/2, -x_k[4]*tstep/2, x_k[1]*tstep/2, -x_k[2]*tstep/2,               1,  x_k[0]*tstep/2]
        Phi_k[6,:] = r_[-x_k[5]*tstep/2,  x_k[4]*tstep/2,  x_k[3]*tstep/2, x_k[2]*tstep/2,  x_k[1]*tstep/2, -x_k[0]*tstep/2,               1]

        # Update error covariance matrix
        #P_k = (eye(7)-K_k*H_k)*P_k
        # Check: why is H_k used in the original formulas?
        P_k = (H_k - K_k) * P_k

        # Projection of state quaternions
        x_k[3:] += quat.quatmult(0.5*x_k[3:],r_[0, x_k[:3]]).flatten()
        x_k[3:] = vector.normalize( x_k[3:] )
        x_k[:3] = np.zeros(3)
        x_k[:3] = tstep * (-x_k[:3]+z_k[:3])

        qOut[ii,:] = x_k[3:]

        # Projection of error covariance matrix
        P_k = Phi_k * P_k * Phi_k.T + Q_k

    # Make the first position the reference position
    qOut = quat.quatmult(qOut, quat.quatinv(qOut[0]))
        
    return qOut
Exemplo n.º 11
0
def calc_QPos(R_initialOrientation, omega, initialPosition, accMeasured, rate):
    ''' Reconstruct position and orientation, from angular velocity and linear acceleration.
    Assumes a start in a stationary position. No compensation for drift.

    Parameters
    ----------
    omega : ndarray(N,3)
        Angular velocity, in [rad/s]
    accMeasured : ndarray(N,3)
        Linear acceleration, in [m/s^2]
    initialPosition : ndarray(3,)
        initial Position, in [m]
    R_initialOrientation: ndarray(3,3)
        Rotation matrix describing the initial orientation of the sensor,
        except a mis-orienation with respect to gravity
    rate : float
        sampling rate, in [Hz]

    Returns
    -------
    q : ndarray(N,3)
        Orientation, expressed as a quaternion vector
    pos : ndarray(N,3)
        Position in space [m]

    Example
    -------
    >>> q1, pos1 = calc_QPos(R_initialOrientation, omega, initialPosition, acc, rate)

    '''

    # Transform recordings to angVel/acceleration in space --------------

    # Orientation of \vec{g} with the sensor in the "R_initialOrientation"
    g = 9.81
    g0 = np.linalg.inv(R_initialOrientation).dot(r_[0,0,g])

    # for the remaining deviation, assume the shortest rotation to there
    q0 = vector.qrotate(accMeasured[0], g0)    
    R0 = quat.quat2rotmat(q0)

    # combine the two, to form a reference orientation. Note that the sequence
    # is very important!
    R_ref = R_initialOrientation.dot(R0)
    q_ref = quat.rotmat2quat(R_ref)

    # Calculate orientation q by "integrating" omega -----------------
    q = quat.vel2quat(omega, q_ref, rate, 'bf')

    # Acceleration, velocity, and position ----------------------------
    # From q and the measured acceleration, get the \frac{d^2x}{dt^2}
    g_v = r_[0, 0, g] 
    accReSensor = accMeasured - vector.rotate_vector(g_v, quat.quatinv(q))
    accReSpace = vector.rotate_vector(accReSensor, q)

    # Make the first position the reference position
    q = quat.quatmult(q, quat.quatinv(q[0]))

    # compensate for drift
    #drift = np.mean(accReSpace, 0)
    #accReSpace -= drift*0.7

    # Position and Velocity through integration, assuming 0-velocity at t=0
    vel = np.nan*np.ones_like(accReSpace)
    pos = np.nan*np.ones_like(accReSpace)

    for ii in range(accReSpace.shape[1]):
        vel[:,ii] = cumtrapz(accReSpace[:,ii], dx=1./rate, initial=0)
        pos[:,ii] = cumtrapz(vel[:,ii],        dx=1./rate, initial=initialPosition[ii])

    return (q, pos)