class BachmannCF(OrientationFilter):
"""
Implementation of the complementary filter algorithm proposed by Bachmann.
See E. R. Bachmann. "Inertial and Magnetic Tracking of Limb Segment
Orientation for Inserting Humans into Synthetic Environments". PhD thesis,
Naval Postgraduate School, California, 2000.
@ivar vectorObservation: L{TimeSeries} of vector observation results.
"""
@prepend_method_doc(OrientationFilter)
def __init__(self, initialTime, initialRotation, k, **kwargs):
"""
@param k: Filter blending co-efficient (float). At each timestep, a
correction will be applied of M{1/k} times the difference between
the gyro integration and vector observation results.
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation)
self._k = float(k)
self.qHat = initialRotation.copy()
self._vectorObservation = vector_observation.FQA()
self.vectorObservation = TimeSeries()
def _update(self, accel, mag, gyro, dt, t):
dotq = 0.5 * self.qHat * Quaternion(0, *gyro)
self.qHat += dotq * dt
qMeas = self._vectorObservation(-accel, mag)
if self.qHat.dot(qMeas) < 0:
qMeas.negate()
qError = qMeas - self.qHat
self.qHat += (1 / self._k) * dt * qError
self.qHat.normalise()
self.vectorObservation.add(t, qMeas)
self.rotation.add(t, self.qHat)
def load_observations(h5_file):
framegroup = h5_file['camera']
frames = sorted(framegroup.keys())
ts = TimeSeries()
for fkey in frames:
group = framegroup[fkey]
obs_dict = {
lm_id: ip.reshape(2, 1)
for lm_id, ip in zip(group['landmarks'].value,
group['measurements'].value.T)
}
t = group['time'].value
ts.add(t, obs_dict)
return ts
class OrientCF(OrientationFilter):
"""
Implementation of the complementary filter used on the Orient IMU.
See A. Young, M. Ling, and D. K. Arvind. "Orient-2: A Realtime Wireless
Posture Tracking System using Local Orientation Estimation". in Proc.
4th Workshop on Embedded Network Sensors, pp 53-57. ACM, 2007.
@ivar vectorObservation: L{TimeSeries} of vector observation results.
"""
@prepend_method_doc(BachmannCF)
def __init__(self, initialTime, initialRotation, k, aT, **kwargs):
"""
@param aT: acceleration threshold (float). The correction step will
only be performed if the condition M{abs(norm(accel) - 1) <= aT}
is met.
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation)
self._k = float(k)
self._aT = float(aT)
self.qHat = initialRotation.copy()
self._vectorObservation = vector_observation.GramSchmidt()
self.vectorObservation = TimeSeries()
def _update(self, accel, mag, gyro, dt, t):
dotq = 0.5 * self.qHat * Quaternion(0,*gyro)
self.qHat += dotq * dt
if abs(vectors.norm(accel) - 1) < self._aT:
qMeas = self._vectorObservation(-accel, mag)
if self.qHat.dot(qMeas) < 0:
qMeas.negate()
qError = qMeas - self.qHat
self.qHat += (1/self._k) * dt * qError
else:
qMeas = Quaternion.nan()
self.vectorObservation.add(t, qMeas)
self.qHat.normalise()
self.rotation.add(t, self.qHat)
class BachmannCF(OrientationFilter):
"""
Implementation of the complementary filter algorithm proposed by Bachmann.
See E. R. Bachmann. "Inertial and Magnetic Tracking of Limb Segment
Orientation for Inserting Humans into Synthetic Environments". PhD thesis,
Naval Postgraduate School, California, 2000.
@ivar vectorObservation: L{TimeSeries} of vector observation results.
"""
@prepend_method_doc(OrientationFilter)
def __init__(self, initialTime, initialRotation, k, **kwargs):
"""
@param k: Filter blending co-efficient (float). At each timestep, a
correction will be applied of M{1/k} times the difference between
the gyro integration and vector observation results.
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation)
self._k = float(k)
self.qHat = initialRotation.copy()
self._vectorObservation = vector_observation.FQA()
self.vectorObservation = TimeSeries()
def _update(self, accel, mag, gyro, dt, t):
dotq = 0.5 * self.qHat * Quaternion(0, *gyro)
self.qHat += dotq * dt
qMeas = self._vectorObservation(-accel, mag)
if self.qHat.dot(qMeas) < 0:
qMeas.negate()
qError = qMeas - self.qHat
self.qHat += (1/self._k) * dt * qError
self.qHat.normalise()
self.vectorObservation.add(t, qMeas)
self.rotation.add(t, self.qHat)
class DistLinAccelCF(OrientationFilter):
"""
Implementation of the multi-IMU orientation algorithm by Young & Ling.
See A. D. Young, M. J. Ling and D. K. Arvind. "Distributed Estimation of
Linear Acceleration for Improved Accuracy in Wireless Inertial Motion
Capture", in Proc. 9th ACM/IEEE International Conference on Information
Processing in Sensor Networks, ACM, 2010, pp. 256-267.
@ivar qMeas: Result of latest vector observation (L{Quaternion}).
@ivar vectorObservation: L{TimeSeries} of vector observation results.
@ivar jointAccel: Latest estimate of joint acceleration
(3x1 L{np.ndarray}).
@ivar jointAcceleration: L{TimeSeries} of acceleration estimates.
"""
GRAVITY_VECTOR = vectors.vector(0, 0, STANDARD_GRAVITY)
@prepend_method_doc(BachmannCF)
def __init__(self, initialTime, initialRotation, k, joint, offset,
**kwargs):
"""
@param joint: The L{Joint} the IMU is attached to.
@param offset: The offset of the IMU in the joint's co-ordinate frame
(3x1 L{np.ndarray})
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation)
self.qHat = initialRotation.copy()
self.k = k
self.joint = joint
self.offset = offset
self._vectorObservation = vector_observation.GramSchmidt()
self.qMeas = Quaternion.nan()
self.jointAccel = vectors.nan()
self.gyroFIFO = collections.deque([], maxlen=3)
self.accelFIFO = collections.deque([], maxlen=2)
self.magFIFO = collections.deque([], maxlen=2)
self.isRoot = not joint.hasParent
self.children = joint.childJoints
self.vectorObservation = TimeSeries()
self.jointAcceleration = TimeSeries()
def handleLinearAcceleration(self, jointAcceleration, dt):
"""
Perform drift correction based on incoming joint acceleration estimate.
@param jointAcceleration: Acceleration estimate (3x1 L{np.ndarray}).
"""
self.jointAccel = jointAcceleration
if len(self.gyroFIFO) < 3:
return
# Estimate linear acceleration
o = self.offset
w = self.gyroFIFO
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = (q.rotateFrame(self.jointAccel) + lt + lr)
# Apply drift correction
self.qMeas = self._vectorObservation(-(self.accelFIFO[1] - l),
self.magFIFO[1])
if q.dot(self.qMeas) < 0:
self.qMeas.negate()
q = q + 1.0 / self.k * dt * (self.qMeas - q)
dotq = 0.5 * self.qHat * Quaternion(0, *w[0])
self.qHat = q + dotq * dt
self.qHat.normalise()
def childAcceleration(self, o, dt):
"""
Compute acceleration for child joint.
@param o: Offset of child joint (3x1 L{np.ndarray}).
"""
w = self.gyroFIFO
if len(w) < 3:
return None
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = self.jointAccel + q.rotateVector(lt + lr)
return l
def _update(self, accel, mag, gyro, dt, t):
# Store measurements in queues for processing
self.gyroFIFO.appendleft(gyro)
self.accelFIFO.appendleft(accel)
self.magFIFO.appendleft(mag)
# save current qHat for drift correction
self.qHat_minus_1 = copy.copy(self.qHat)
# Inertial update
dotq = 0.5 * self.qHat * Quaternion(0, *gyro)
self.qHat += dotq * dt
self.qHat.normalise()
if self.isRoot and len(self.accelFIFO) >= 2:
# Root can't receive acceleration data so must estimate it.
measuredAccel = self.qHat_minus_1.rotateVector(self.accelFIFO[1])
# Assume root joint acceleration was zero.
self.jointAccel = np.zeros((3, 1))
# Calculate tangential acceleration for root joint IMU offset.
linAccel = self.childAcceleration(self.offset, dt)
if linAccel is not None:
# Calculate expected measurement.
expectedAccel = linAccel - self.GRAVITY_VECTOR
# Set root joint acceleration to the residual.
self.jointAccel = measuredAccel - expectedAccel
self.handleLinearAcceleration(self.jointAccel, dt)
self.rotation.add(t, self.qHat)
self.vectorObservation.add(t, self.qMeas)
self.jointAcceleration.add(t, self.jointAccel)
class YunEKF(OrientationFilter):
"""
Implementation of the extented Kalman filter proposed by Yun et al.
See X. Yun and E. R. Bachmann. "Design, Implementation, and Experiemental
Results of a Quaternion-Based Kalman Filter for Human Body Motion
Tracking". IEEE Transactions on Robotics, vol 22, no 6, pp 1216-1227, 2006.
@ivar vectorObservation: L{TimeSeries} of vector observation results.
@ivar rotationalVelocity: L{TimeSeries} of angular rate estimates.
"""
@prepend_method_doc(OrientationFilter)
def __init__(self, initialTime, initialRotation, initialRotationalVelocity,
initialCovariance, measurementCovariance, D, tau, **kwargs):
"""
@param initialRotationalVelocity: Initial angular rate estimate
(3x1 L{np.ndarray}).
@param initialCovariance: Initial state covariance matrix (7x7).
@param measurementCovariance: Measurement noise covariance (7x7).
@param D: Variance of process noise model in rad^2/s^2 (float).
@param tau: Time constant of process noise model in seconds (float).
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation, initialCovariance[3:,
3:])
self.tau = float(tau)
self.D = float(D)
self._vectorObservation = vector_observation.FQA()
# Process noise covariance matrix, updated on each call.
self.Q = np.asmatrix(np.zeros((7, 7)))
# Measurement noise covariance matrix
self.R = measurementCovariance
# Prior covariance estimate and state vector initialisation.
self.P_minus = initialCovariance
self.xHat_minus = np.vstack(
(initialRotationalVelocity, np.vstack(initialRotation.components)))
self.vectorObservation = TimeSeries()
self.rotationalVelocity = TimeSeries()
def _update(self, accel, mag, gyro, dt, t):
# Note: Measurement matrix H is equivalent to the identity matrix
# so is left out to simplify calculations
# Calculate Kalman gain
K = self.P_minus * np.linalg.inv(self.P_minus + self.R)
# Construct measurment vector z
self.vo = self._vectorObservation(-accel, mag)
if self.vo.dot(Quaternion(*self.xHat_minus[3:])) < 0:
self.vo.negate()
z = np.vstack((gyro, np.vstack(self.vo.components)))
# Calculate state update
xHat = self.xHat_minus + K * (z - self.xHat_minus)
P = (np.eye(7) - K) * self.P_minus
# Normalise quaternion component of state vector
qHat = Quaternion(*xHat[3:]).normalise()
xHat[3:] = np.vstack(qHat.components)
# Calculate prediction
self.xHat_minus = xHat + self.dotx(xHat) * dt
Phi = self.Phi(xHat, dt)
self.Q[[0,1,2],[0,1,2]] = \
(self.D / (2 * self.tau)) * (1 - math.e**((-2 * dt) / self.tau))
self.P_minus = Phi * P * Phi.T + self.Q
self.rotation.add(t, qHat, P[3:, 3:])
self.vectorObservation.add(t, self.vo)
self.rotationalVelocity.add(t, xHat[:3], P[:3, :3])
def dotx(self, xHat):
"""
Calculate state derivative vector.
"""
dotx = np.empty((7, 1))
dotx[:3] = -xHat[:3] / self.tau
dotx[3] = -0.5 * (xHat[0] * xHat[4] + xHat[1] * xHat[5] +
xHat[2] * xHat[6])
dotx[4] = 0.5 * (xHat[0] * xHat[3] - xHat[1] * xHat[6] +
xHat[2] * xHat[5])
dotx[5] = 0.5 * (xHat[0] * xHat[6] + xHat[1] * xHat[3] -
xHat[2] * xHat[4])
dotx[6] = 0.5 * (-xHat[0] * xHat[5] + xHat[1] * xHat[4] +
xHat[2] * xHat[3])
return dotx
def Phi(self, xHat, dt):
"""
Calculate discrete state transition matrix Phi.
"""
# Note: Yun paper matrix element indices are one-based so subtract 1
# from all for zero-based indices.
Phi = np.eye(7)
Phi[0, 0] = math.e**(-dt / self.tau)
Phi[1, 1] = math.e**(-dt / self.tau)
Phi[2, 2] = math.e**(-dt / self.tau)
Phi[3, 0] = -xHat[4] * dt / 2
Phi[3, 1] = -xHat[5] * dt / 2
Phi[3, 2] = -xHat[6] * dt / 2
Phi[3, 4] = -xHat[0] * dt / 2
Phi[3, 5] = -xHat[1] * dt / 2
Phi[3, 6] = -xHat[2] * dt / 2
Phi[4, 0] = xHat[3] * dt / 2
Phi[4, 1] = -xHat[6] * dt / 2
Phi[4, 2] = xHat[5] * dt / 2
Phi[4, 2] = xHat[0] * dt / 2
Phi[4, 5] = xHat[2] * dt / 2
Phi[4, 6] = -xHat[1] * dt / 2
Phi[5, 0] = xHat[6] * dt / 2
Phi[5, 1] = xHat[3] * dt / 2
Phi[5, 2] = -xHat[4] * dt / 2
Phi[5, 3] = xHat[1] * dt / 2
Phi[5, 4] = -xHat[2] * dt / 2
Phi[5, 6] = xHat[0] * dt / 2
Phi[6, 0] = -xHat[5] * dt / 2
Phi[6, 1] = xHat[4] * dt / 2
Phi[6, 2] = xHat[3] * dt / 2
Phi[6, 3] = xHat[2] * dt / 2
Phi[6, 4] = xHat[1] * dt / 2
Phi[6, 5] = -xHat[0] * dt / 2
return Phi
class DistLinAccelCF(OrientationFilter):
"""
Implementation of the multi-IMU orientation algorithm by Young & Ling.
See A. D. Young, M. J. Ling and D. K. Arvind. "Distributed Estimation of
Linear Acceleration for Improved Accuracy in Wireless Inertial Motion
Capture", in Proc. 9th ACM/IEEE International Conference on Information
Processing in Sensor Networks, ACM, 2010, pp. 256-267.
@ivar qMeas: Result of latest vector observation (L{Quaternion}).
@ivar vectorObservation: L{TimeSeries} of vector observation results.
@ivar jointAccel: Latest estimate of joint acceleration
(3x1 L{np.ndarray}).
@ivar jointAcceleration: L{TimeSeries} of acceleration estimates.
"""
GRAVITY_VECTOR = vectors.vector(0, 0, STANDARD_GRAVITY)
@prepend_method_doc(BachmannCF)
def __init__(self, initialTime, initialRotation, k, joint, offset,
**kwargs):
"""
@param joint: The L{Joint} the IMU is attached to.
@param offset: The offset of the IMU in the joint's co-ordinate frame
(3x1 L{np.ndarray})
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation)
self.qHat = initialRotation.copy()
self.k = k
self.joint = joint
self.offset = offset
self._vectorObservation = vector_observation.GramSchmidt()
self.qMeas = Quaternion.nan()
self.jointAccel = vectors.nan()
self.gyroFIFO = collections.deque([], maxlen=3)
self.accelFIFO = collections.deque([], maxlen=2)
self.magFIFO = collections.deque([], maxlen=2)
self.isRoot = not joint.hasParent
self.children = joint.childJoints
self.vectorObservation = TimeSeries()
self.jointAcceleration = TimeSeries()
def handleLinearAcceleration(self, jointAcceleration, dt):
"""
Perform drift correction based on incoming joint acceleration estimate.
@param jointAcceleration: Acceleration estimate (3x1 L{np.ndarray}).
"""
self.jointAccel = jointAcceleration
if len(self.gyroFIFO) < 3:
return
# Estimate linear acceleration
o = self.offset
w = self.gyroFIFO
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = (q.rotateFrame(self.jointAccel) + lt + lr)
# Apply drift correction
self.qMeas = self._vectorObservation(-(self.accelFIFO[1]- l),
self.magFIFO[1])
if q.dot(self.qMeas) < 0:
self.qMeas.negate()
q = q + 1.0/self.k * dt * (self.qMeas - q)
dotq = 0.5 * self.qHat * Quaternion(0,*w[0])
self.qHat = q + dotq * dt
self.qHat.normalise()
def childAcceleration(self, o, dt):
"""
Compute acceleration for child joint.
@param o: Offset of child joint (3x1 L{np.ndarray}).
"""
w = self.gyroFIFO
if len(w) < 3:
return None
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = self.jointAccel + q.rotateVector(lt + lr)
return l
def _update(self, accel, mag, gyro, dt, t):
# Store measurements in queues for processing
self.gyroFIFO.appendleft(gyro)
self.accelFIFO.appendleft(accel)
self.magFIFO.appendleft(mag)
# save current qHat for drift correction
self.qHat_minus_1 = copy.copy(self.qHat)
# Inertial update
dotq = 0.5 * self.qHat * Quaternion(0,*gyro)
self.qHat += dotq * dt
self.qHat.normalise()
if self.isRoot and len(self.accelFIFO) >= 2:
# Root can't receive acceleration data so must estimate it.
measuredAccel = self.qHat_minus_1.rotateVector(self.accelFIFO[1])
# Assume root joint acceleration was zero.
self.jointAccel = np.zeros((3,1))
# Calculate tangential acceleration for root joint IMU offset.
linAccel = self.childAcceleration(self.offset, dt)
if linAccel is not None:
# Calculate expected measurement.
expectedAccel = linAccel - self.GRAVITY_VECTOR
# Set root joint acceleration to the residual.
self.jointAccel = measuredAccel - expectedAccel
self.handleLinearAcceleration(self.jointAccel, dt)
self.rotation.add(t, self.qHat)
self.vectorObservation.add(t, self.qMeas)
self.jointAcceleration.add(t, self.jointAccel)
class YunEKF(OrientationFilter):
"""
Implementation of the extented Kalman filter proposed by Yun et al.
See X. Yun and E. R. Bachmann. "Design, Implementation, and Experiemental
Results of a Quaternion-Based Kalman Filter for Human Body Motion
Tracking". IEEE Transactions on Robotics, vol 22, no 6, pp 1216-1227, 2006.
@ivar vectorObservation: L{TimeSeries} of vector observation results.
@ivar rotationalVelocity: L{TimeSeries} of angular rate estimates.
"""
@prepend_method_doc(OrientationFilter)
def __init__(self, initialTime, initialRotation, initialRotationalVelocity,
initialCovariance, measurementCovariance, D, tau, **kwargs):
"""
@param initialRotationalVelocity: Initial angular rate estimate
(3x1 L{np.ndarray}).
@param initialCovariance: Initial state covariance matrix (7x7).
@param measurementCovariance: Measurement noise covariance (7x7).
@param D: Variance of process noise model in rad^2/s^2 (float).
@param tau: Time constant of process noise model in seconds (float).
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation, initialCovariance[3:,3:])
self.tau = float(tau)
self.D = float(D)
self._vectorObservation = vector_observation.FQA()
# Process noise covariance matrix, updated on each call.
self.Q = np.asmatrix(np.zeros((7,7)))
# Measurement noise covariance matrix
self.R = measurementCovariance
# Prior covariance estimate and state vector initialisation.
self.P_minus = initialCovariance
self.xHat_minus = np.vstack((initialRotationalVelocity,
np.vstack(initialRotation.components)))
self.vectorObservation = TimeSeries()
self.rotationalVelocity = TimeSeries()
def _update(self, accel, mag, gyro, dt, t):
# Note: Measurement matrix H is equivalent to the identity matrix
# so is left out to simplify calculations
# Calculate Kalman gain
K = self.P_minus * np.linalg.inv(self.P_minus + self.R)
# Construct measurment vector z
self.vo = self._vectorObservation(-accel, mag)
if self.vo.dot(Quaternion(*self.xHat_minus[3:])) < 0:
self.vo.negate()
z = np.vstack((gyro, np.vstack(self.vo.components)))
# Calculate state update
xHat = self.xHat_minus + K * (z - self.xHat_minus)
P = (np.eye(7) - K) * self.P_minus
# Normalise quaternion component of state vector
qHat = Quaternion(*xHat[3:]).normalise()
xHat[3:] = np.vstack(qHat.components)
# Calculate prediction
self.xHat_minus = xHat + self.dotx(xHat) * dt
Phi = self.Phi(xHat, dt)
self.Q[[0,1,2],[0,1,2]] = \
(self.D / (2 * self.tau)) * (1 - math.e**((-2 * dt) / self.tau))
self.P_minus = Phi * P * Phi.T + self.Q
self.rotation.add(t, qHat, P[3:,3:])
self.vectorObservation.add(t, self.vo)
self.rotationalVelocity.add(t, xHat[:3], P[:3,:3])
def dotx(self, xHat):
"""
Calculate state derivative vector.
"""
dotx = np.empty((7, 1))
dotx[:3] = -xHat[:3] / self.tau
dotx[3] = -0.5 * (xHat[0]*xHat[4] + xHat[1]*xHat[5] + xHat[2]*xHat[6])
dotx[4] = 0.5 * (xHat[0]*xHat[3] - xHat[1]*xHat[6] +xHat[2]*xHat[5])
dotx[5] = 0.5 * (xHat[0]*xHat[6] + xHat[1]*xHat[3] - xHat[2]*xHat[4])
dotx[6] = 0.5 * (-xHat[0]*xHat[5] +xHat[1]*xHat[4] + xHat[2]*xHat[3])
return dotx
def Phi(self, xHat, dt):
"""
Calculate discrete state transition matrix Phi.
"""
# Note: Yun paper matrix element indices are one-based so subtract 1
# from all for zero-based indices.
Phi = np.eye(7)
Phi[0,0] = math.e**(-dt / self.tau)
Phi[1,1] = math.e**(-dt / self.tau)
Phi[2,2] = math.e**(-dt / self.tau)
Phi[3,0] = -xHat[4] * dt / 2
Phi[3,1] = -xHat[5] * dt / 2
Phi[3,2] = -xHat[6] * dt / 2
Phi[3,4] = -xHat[0] * dt / 2
Phi[3,5] = -xHat[1] * dt / 2
Phi[3,6] = -xHat[2] * dt / 2
Phi[4,0] = xHat[3] * dt / 2
Phi[4,1] = -xHat[6] * dt / 2
Phi[4,2] = xHat[5] * dt / 2
Phi[4,2] = xHat[0] * dt / 2
Phi[4,5] = xHat[2] * dt / 2
Phi[4,6] = -xHat[1] * dt / 2
Phi[5,0] = xHat[6] * dt / 2
Phi[5,1] = xHat[3] * dt / 2
Phi[5,2] = -xHat[4] * dt / 2
Phi[5,3] = xHat[1] * dt / 2
Phi[5,4] = -xHat[2] * dt / 2
Phi[5,6] = xHat[0] * dt / 2
Phi[6,0] = -xHat[5] * dt / 2
Phi[6,1] = xHat[4] * dt / 2
Phi[6,2] = xHat[3] * dt / 2
Phi[6,3] = xHat[2] * dt / 2
Phi[6,4] = xHat[1] * dt / 2
Phi[6,5] = -xHat[0] * dt / 2
return Phi
