def update(self):
# Measure raw values
self.measure()
# Convert gyroscope values to quaternion matrix
(_p, _q, _r) = self.filter_gyro()
_p = _p * 0.01 / 2
_q = _q * 0.01 / 2
_r = _r * 0.01 / 2
_A = matrix.matrix(4, 4, data=[[1, -_p, -_q, -_r],
[_p, 1, _r, -_q],
[_q, -_r, 1, _p],
[_r, _q, -_p, 1]])
# Filter raw accelerometer values
(_Ax, _Ay, _Az) = self.filter_accel()
# Filter raw magnetometer values
(_Mx, _My, _Mz) = self.filter_mag()
# Calculate roll, pitch and yaw
(_roll, _pitch, _yaw) = self.attitude(_Ax, _Ay, _Az, _Mx, _My, _Mz)
# Convert the filtered values to quaternion
_z = self.quatern(_roll, _pitch, _yaw)
# Filter the values
self.kalman_filter.update(_z, _A)
# Return the current filtered values
return self.kalman_filter.get()
def __init__(self, i2c, ak8963=None):
# Initialise the sensor
self.sensor = MPU9250(i2c, ak8963=ak8963)
# Initialise the class variables
self.G_roll = 0
self.G_pitch = 0
self.G_yaw = 0
# Offset values for the gyroscope output
self._gyroOffset = (-83.53642,78.94066,-69.39014)
self._gyroScale = (0.1, 0.1, 0.1)
# Initialise the filters for the accelerometer and gyroscope
_a = [1.0000, -1.8879, 0.8946] # Denominator coefficients
# Gain
_K = sum(_a) / 4
_b = [1.0000*_K, 1.9701*_K, 0.9703*_K] # Numerator coefficients
self.accel_filter_x = iir(_a, _b)
self.accel_filter_y = iir(_a, _b)
self.accel_filter_z = iir(_a, _b)
self.mag_filter_x = iir(_a, _b)
self.mag_filter_y = iir(_a, _b)
self.mag_filter_z = iir(_a, _b)
self.gyro_filter_x = iir(_a, _b)
self.gyro_filter_y = iir(_a, _b)
self.gyro_filter_z = iir(_a, _b)
# Initialise Kalman filter matrices
_q = 0.0001 # Process noise
_r = 10 # Signal noise
_Q = matrix.matrix(4, 4, data=[[_q, 0, 0, 0],
[0, _q, 0, 0],
[0, 0, _q, 0],
[0, 0, 0, _q]])
_R = matrix.matrix(4, 4, data=[[_r, 0, 0, 0],
[0, _r, 0, 0],
[0, 0, _r, 0],
[0, 0, 0, _r]])
# Create the Kalman filter
self.kalman_filter = eulerKalman(_Q, _R)
def __init__(self, Q, R, n=1):
# Q - Process noise variance - should be a 4 x 4 matrix
# R - Signal noise variance - also a 4 x 4 matrix
# n - Dimension of the Kalman filter
self._Q = Q
self._R = R
# Initialise the filter variables
self._x = matrix.matrix(n, 1) # Matrix to hold the quaternion values
self._P = matrix.eye(n) # 4 x 4 identity matrix
self._K = 0 # Shouldn't matter, as it is calculated fresh each iteration
def getQuatern():
q1 = 0 + noise(0.1)
q2 = 0 + noise(0.1)
q3 = 0 + noise(0.1)
q4 = 0 + noise(0.1)
data = [[q1],
[q2],
[q3],
[q4]]
Q = matrix.matrix(4, 1, data)
return Q
def __init__(self, Q, R):
# Q - Process noise variance - should be a 4 x 4 matrix
# R - Signal noise variance - also a 4 x 4 matrix
# n - Dimension of the Kalman filter
self._Q = Q
self._R = R
# Initialise the filter variables
self._x = matrix.matrix(4, 1) # Matrix to hold the quaternion values
self._x.store(1, 0, 0)
self._P = matrix.eye(4) # 4 x 4 identity matrix
self._K = 0 # Shouldn't matter, as it is calculated fresh each iteration
# Initialise the roll, pitch and yaw
self._roll = 0
self._pitch = 0
self._yaw = 0
def quatern(self, roll=0, pitch=0, yaw=0):
# Calculate intermediate values
r1 = math.sin(roll / 2)
r2 = math.cos(roll / 2)
p1 = math.sin(pitch / 2)
p2 = math.cos(pitch / 2)
y1 = math.sin(yaw / 2)
y2 = math.cos(yaw / 2)
# z = [ cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi,
# sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi,
# cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi,
# cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi]
_data = [[r2 * p2 * y2 + r1 * p1 * y1], [r1 * p2 * y2 + r2 * p1 * y1],
[r2 * p1 * y2 + r1 * p2 * y1], [r2 * p2 * y1 + r1 * p1 * y2]]
_z = matrix.matrix(4, 1, _data)
return _z
# Create the dummy dataset
N = 1024 # Number of samples
Fs = 500 # Sample rate (samples/sec)
Ts = 1 / Fs # Sample period (sec)
# Time variable
t = np.linspace(0.0, N*Ts, N)
# Example output - two sinusoids
# x = list(np.sin(5.0 * 2.0*np.pi*t) + 0.5*np.sin(2.0 * 2.0*np.pi*t))
# Initialise Kalman filter matrices
q = 0.0001 # Process noise
r = 10 # Signal noise
Q = matrix.matrix(4, 4, data=[[q, 0, 0, 0],
[0, q, 0, 0],
[0, 0, q, 0],
[0, 0, 0, q]])
R = matrix.matrix(4, 4, data=[[r, 0, 0, 0],
[0, r, 0, 0],
[0, 0, r, 0],
[0, 0, 0, r]])
# Initialise the filter
filter2 = eulerKalman(Q, R)
# Initialise the lists
Gx = []
Gy = []
Gz = []
Ax = []
Ay = []
