def calc_orientation(gyr, acc, samplerate, align_arr, beta, delta_t, times): ref_qq = np.zeros([len(acc), 4]) ref_eu = np.zeros([len(acc), 3]) sampleperiod = 1 / samplerate qq = Quaternion(align_arr) madgwick = MadgwickAHRS(sampleperiod=sampleperiod, quaternion=qq, beta=beta) count = 0 ll = len(gyr) for i in range(0, len(gyr) - 1): if (i % 300) == 0: print( str(i) + ' / ' + str(ll) + ' ' + str(round(i / ll * 100, 0)) + '%') while (count < times): madgwick.update_imu(gyr[i], acc[i], delta_t[i]) count += 1 count = 0 # ref_qq[i]=madgwick.quaternion.elements # ref_eu[i]=madgwick.quaternion.eu_angle ref_qq[i, 0] = madgwick.quaternion._get_q()[0] ref_qq[i, 1] = madgwick.quaternion._get_q()[1] ref_qq[i, 2] = madgwick.quaternion._get_q()[2] ref_qq[i, 3] = madgwick.quaternion._get_q()[3] tempq = Quaternion(ref_qq[i]) ref_eu[i, :] = tempq.to_euler_angles_by_wiki() ref_eu[i] *= 180 / np.pi return ref_qq, ref_eu
def calc_orientation_mag(gyr, acc, mag, samplerate, align_arr, beta, delta_t, times): ref_qq = np.zeros([len(acc), 4]) ref_eu = np.zeros([len(acc), 3]) # out_qq=np.zeros([len(acc),4]) # out_eu=np.zeros([len(acc),3]) sampleperiod = 1 / samplerate qq = align_arr madgwick = MadgwickAHRS(sampleperiod=sampleperiod, quaternion=qq, beta=beta) rms_mag = np.zeros(len(mag)) for i in range(len(mag)): rms_mag[i] = np.sqrt( np.mean(mag[i, 0]**2 + mag[i, 1]**2 + mag[i, 2]**2)) count = 0 ll = len(gyr) for i in range(0, len(gyr) - 1): if (i % 2000) == 0: print( str(i) + ' / ' + str(ll) + ' ' + str(round(i / ll * 100, 1)) + '%') warnings.warn('mag maxium limit is on') if (rms_mag[i] > 999): print('warning!! magnetometer warning') warnings.warn('rms_mag[i]>maxium') while (count < times): madgwick.update_imu(gyr[i], acc[i], delta_t[i]) count += 1 count = 0 else: while (count < times): madgwick.update(gyr[i], acc[i], mag[i], delta_t[i]) count += 1 count = 0 ref_qq[i, 0] = madgwick.quaternion._get_q()[0] ref_qq[i, 1] = madgwick.quaternion._get_q()[1] ref_qq[i, 2] = madgwick.quaternion._get_q()[2] ref_qq[i, 3] = madgwick.quaternion._get_q()[3] # ref_qq[i]=madgwick.quaternion.elements tempq = Quaternion(ref_qq[i]) # ref_eu[i, :] = tempq.to_euler_angles_by_wiki() ref_eu[i] *= 180 / np.pi # ref_eu[i]=madgwick.quaternion.eu_angle # return ref_qq, ref_eu
def main(): new_data = MadgwickAHRS() while 1: gyr = np.array(gyro.Get_CalOut_Value()) acc = np.array(accel.getAccel()) while not bool(magnet.isMagReady()): print "not ready" time.sleep(0.1) mag = np.array(magnet.getMag()) gyr_rad = gyr * (np.pi / 180) new_data.update_imu(gyr_rad, acc) print(gyr, acc, mag) print "---------------------------\n" ahrs = new_data.quaternion.to_euler_angles() print ahrs time.sleep(1)
(gyro_scaled_x, gyro_scaled_y, gyro_scaled_z) = s.Get_CalOut_Value() (accel_scaled_x, accel_scaled_y, accel_scaled_z) = ss.getAccel() last_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z) last_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z) gyro_offset_x = gyro_scaled_x gyro_offset_y = gyro_scaled_y gyro_total_x = (last_x) - gyro_offset_x gyro_total_y = (last_y) - gyro_offset_y return gyro_total_x, gyro_total_y, gyro_offset_x, gyro_offset_y, last_x, last_y if __name__ == "__main__": new_data = MadgwickAHRS() while 1: gyr = np.array(s.Get_CalOut_Value()) acc = np.array(ss.getAccel()) print(gyr) # while not bool(sm.isMagReady()): # print "not ready" # time.sleep(0.1) # mag = np.array(sm.getMag()) gyr_rad = gyr * (np.pi / 180) new_data.update_imu(gyr_rad, acc) ahrs = new_data.quaternion.to_euler_angles() # print ahrs time.sleep(1)
# ------------------------------- # Initiate AHRS refq = Quaternion(1,0,0,0) # look front #refq = Quaternion(axis=[0,0,1],angle=np.pi/2) # look right #refq = Quaternion(axis=[1,0,0],angle=np.pi/2) # look front, roll 90' left-down #refq = Quaternion(axis=[0,1,0],angle=np.pi/2) # look up ahrs = MadgwickAHRS(sampleperiod=0.0005, beta=0.1, quaternion=refq) while Sensor or options.sim: if options.sim: # use fake data for testing #ahrs.update_imu((1, 0, 0), (0, 0, 1)) # sideways, pitching #ahrs.update_imu((0, 0, 1), (1, 0, 0)) # looking front, yawing to right ahrs.update_imu((0, 1, 0), (0, 1, 0)) # looking right, pitching up else: data = Sensor.read(64) if data == None: continue frame = sensor.parse(data[16:32]) time1 = frame["timestamp"] if time1 < time2: time2 = time2 - (1 << 24) if time2: delta = (time1 - time2) / 1000000 else: # Assume 500us for first sample delta = 500 / 1000000
plt.xlabel('Time (s)') # ------------------------------------------------------------------------- # Compute orientation from madgwickahrs import MadgwickAHRS quat = [None] * len(time) AHRSalgorithm = MadgwickAHRS(sampleperiod=1 / Fs) # Initial convergence initPeriod = tempo_parado # usually 2 seconds indexSel = time < (tempo_parado + time[0]) for i in range(2000): AHRSalgorithm.update_imu( [0, 0, 0], [accX[indexSel].mean(), accY[indexSel].mean(), accZ[indexSel].mean()]) # [magX[indexSel].mean(), magY[indexSel].mean(), magZ[indexSel].mean()]) # For all data for t in range(len(time)): if stationary[t]: AHRSalgorithm.beta = 0.5 else: AHRSalgorithm.beta = 0 AHRSalgorithm.update_imu(np.deg2rad([gyrX[t], gyrY[t], gyrZ[t]]), [accX[t], accY[t], accZ[t]]) quat[t] = AHRSalgorithm.quaternion quats = []
acc = [] gyro = [] while True: #start = utime.ticks_ms() accelerometer = sensor.accel gyrometer = sensor.gyro acc = umatrix.matrix([accelerometer.x, accelerometer.y, accelerometer.z], cstride=1, rstride=3, dtype=float) gyro = umatrix.matrix([gyrometer.x, gyrometer.y, gyrometer.z], cstride=1, rstride=3, dtype=float) #print("acceleration [X:%0.2f, Y:%0.2f, Z:%0.2f] gyro [X:%0.2f, Y:%0.2f, Z:%0.2f] \n" # %(accelerometer.x, accelerometer.y, accelerometer.z, gyrometer.x * math.pi/180, gyrometer.y * math.pi/180, gyrometer.z * math.pi/180)) #print("temperature [T:%0.2f]\n" %(sensor.temperature)) ahrs.update_imu(gyro * math.pi / 180.0, acc) (roll, pitch, yaw) = ahrs.quaternion.to_euler() print("roll: %0.2f, pitch: %0.2f yaw: %0.2f\n" % (roll, pitch, yaw)) servo0.angle(math.pi / 2 - pitch) servo1.angle(math.pi / 2 + yaw) #print("Elapsed: %d" % (utime.ticks_ms() - start))
throw_ind = (np.where(throw_states == 1))[0][0] except: print("No throw detected") a_values = np.vstack((ax_values_h, ay_values_h, az_values_h)) g_values = np.vstack((gx_values_h, gy_values_h, gz_values_h)) m_values = np.vstack((mx_values_h, my_values_h, mz_values_h)) # Use the Madgwick AHRS filter ahrs = MadgwickAHRS() R = np.zeros((m, 3, 3)) Z_flip = [[1, 0, 0], [0, 1, 0], [0, 0, -1]] for i in range(m): ahrs.update_imu(g_values[:, i], a_values[:, i]) #ahrs.update(g_values[:, i], a_values[:, i], m_values[:, i]) R[i, :, :] = Rotation.from_quat(ahrs.quaternion._q).as_matrix() # Compute the tilt compensated acceleration and subtract gravity tc_a_values = np.zeros((3, m)) for i in range(m): tc_a_values[:, i] = np.dot(R[i, :, :], a_values[:, i]) a_values_final = tc_a_values - np.vstack( (np.zeros(m), np.zeros(m), np.ones(m) * 9.81)) # Integrate acceleration to get velocity, then high pass filter
#start estimation loop for i in range(start, len(imu_df)): #get current imu point imu_point = imu_df.iloc[i] acceleration = imu_point[['accel_x', 'accel_y', 'accel_z' ]].values / 16384 * 9.8 omega = imu_point[['gyro_x', 'gyro_y', 'gyro_z' ]].values / 65535 * 2 * 250 * np.pi / 180 time = imu_point['timestamp_optitrack'] if time > 0: if estimator.is_valid_time(time): #madgwick attitude estimator comp_filt.update_imu(omega, acceleration) comp_quat = comp_filt.quaternion comp_rot = Rotation(comp_quat[0], comp_quat[1], comp_quat[2], comp_quat[3]) #sensor to earth humcomp_rot = Rotation(1, 0, 0, 0).from_rotation_matrix( np.linalg.inv( comp_rot.to_rotation_matrix())) #earth to sensor comp_eulers.append(comp_rot.to_euler_YPR()) rotated_grav = comp_rot.to_rotation_matrix() @ np.array( [0, 0, 9.8]).reshape((3, 1)) #predict step rot_idx = np.searchsorted(gnd_time, time) if rot_idx < len(rotations): rot_mat = rotations[rot_idx].to_rotation_matrix() else: