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
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
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()
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)
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)
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)
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
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)
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)
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
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)