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 = []