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
xm_0=xm_0_6, Pm_0=Pm_0_6, time=time, R_eo=R_eo, gnd_to_lh1_pose=gnd_to_lh1_pose, gnd_to_lh2_pose=gnd_to_lh2_pose, R_init=rotations[start].to_rotation_matrix() @ R_eo, t0=time[start - 1], Rg=20, lh1_gnd=np.linalg.inv(R_eo) @ lh1, lh2_gnd=np.linalg.inv(R_eo) @ lh2, gravity=False) for i in range(start, len(time) - 1): comp_filt.update(omega[i], acceleration[i], compass[i]) comp_quat = comp_filt.quaternion comp_rot = Rotation(comp_quat[0], comp_quat[1], comp_quat[2], comp_quat[3]) #sensor to earth comp_rot = Rotation(1, 0, 0, 0).from_rotation_matrix( np.linalg.inv(comp_rot.to_rotation_matrix())) #earth to sensor rotated_grav = comp_rot.to_rotation_matrix() @ np.array( [0, 0, 9.8]).reshape((3, 1)) comp_eulers.append(comp_rot.to_euler_YPR()) #predict from past state using rotation and acceleration in body axis ''' #KF estimator.predict(i,comp_rot,acceleration[i].reshape((3,1)) - rotated_grav ) measurement = estimator.meas_available(time[i],lh_traj) print(measurement)
gyro_z = read_gyro_z(address) bes_x = read_bes_x(address) bes_y = read_bes_y(address) bes_z = read_bes_z(address) mag_x = read_mag_x(mag_address) mag_y = read_mag_y(mag_address) mag_z = read_mag_z(mag_address) bes_arr = [bes_x, bes_y , bes_z] gyro_arr = [gyro_x, gyro_y, gyro_z] mag_arr = [mag_x, mag_y, mag_z] q = Quaternion(1,0,0,0) m = MadgwickAHRS(1/1000,q,1) #print("before: ",m.quaternion.q) MadgwickAHRS.update(m,bes_arr, gyro_arr, mag_arr) #print("after: ",m.quaternion.q) roll = math.atan2(2.0*(m.quaternion.q[2] * m.quaternion.q[3] + m.quaternion.q[0] * m.quaternion.q[1]), m.quaternion.q[0] * m.quaternion.q[0] - m.quaternion.q[1] * m.quaternion.q[1] - m.quaternion.q[2] * m.quaternion.q[2] + m.quaternion.q[3] * m.quaternion.q[3]) * (180.0 / math.pi) pitch = math.asin(-2.0 * (m.quaternion.q[1] * m.quaternion.q[3] - m.quaternion.q[0] * m.quaternion.q[2])) * (180.0 / math.pi) yaw = math.atan2(2.0*(m.quaternion.q[1]*m.quaternion.q[2] + m.quaternion.q[0] * m.quaternion.q[3]), m.quaternion.q[0] * m.quaternion.q[0] + m.quaternion.q[1] * m.quaternion.q[1] - m.quaternion.q[2] * m.quaternion.q[2] - m.quaternion.q[3] * m.quaternion.q[3]) * (180.0 / math.pi) rotate_roll = np.array([[1,0,0],[0,np.cos(roll),-np.sin(roll)],[0,np.sin(roll),np.cos(roll)]]) rotate_pitch = np.array([[np.cos(pitch),0,np.sin(pitch)],[0,1,0],[-np.sin(pitch),0,np.cos(pitch)]]) rotate_yaw = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]]) print("qua:", m.quaternion.q[0], m.quaternion.q[1], m.quaternion.q[2], m.quaternion.q[3]) print("roll:",roll) print("pitch:",pitch) print("yaw:",yaw) print("")
magYInterpolated = np.interp(magTimeStampInterp, magTimeStamp, magY) magZInterpolated = np.interp(magTimeStampInterp, magTimeStamp, magZ) dt = [(gpsTimeStampInterp[i + 1] - gpsTimeStampInterp[i]) * 0.000000001 for i in range(len(gpsTimeStampInterp) - 1)] dt.insert(0, 0) accXAbsolute = [] accYAbsolute = [] print("Calculating absolute acc values...") heading = MadgwickAHRS(sampleperiod=mean(dt)) for i in range(samples): gyroscope = [gyroZInterpolated[i], gyroYInterpolated[i], gyroXInterpolated[i]] accelerometer = [accZInterpolated[i], accYInterpolated[i], accXInterpolated[i]] magnetometer = [magZInterpolated[i], magYInterpolated[i], magXInterpolated[i]] heading.update(gyroscope, accelerometer, magnetometer) ahrs = heading.quaternion.to_euler_angles() roll = ahrs[0] pitch = ahrs[1] yaw = ahrs[2] + (3.0 * (math.pi / 180.0)) # adding magenetic declination ACC = np.array([[accZ[i]], [accY[i]], [accX[i]]]) ACCABS = np.linalg.inv(rotaitonMatrix(yaw, pitch, roll)).dot(ACC) accXAbsolute.append(-1 * ACCABS[0, 0]) accYAbsolute.append(-1 * ACCABS[1, 0]) # Transition matrix F_t = np.array([[1, 0, dt[0], 0], [0, 1, 0, dt[0]], [0, 0, 1, 0], [0, 0, 0, 1]]) # Initial State cov P_t = np.identity(4) * 400 # Process cov Q_t = np.array([[1000, 0, 100, 0], [0, 1000, 0, 100], [100, 0, 1000, 0], [0, 100, 0, 1000]]) * 0.65
import board import busio import adafruit_fxos8700 import adafruit_fxas21002c from time import sleep from math import atan, sqrt import numpy as np from madgwickahrs import MadgwickAHRS i2c = busio.I2C(board.SCL, board.SDA) fxos = adafruit_fxos8700.FXOS8700(i2c) fxas = adafruit_fxas21002c.FXAS21002C(i2c) '''while True: print('Acceleration (m/s^2): ({0:0.3f},{1:0.3f},{2:0.3f})'.format(*fxos.accelerometer)) print('Magnometer (uTesla): ({0:0.3f},{1:0.3f},{2:0.3f})'.format(*fxos.magnetometer)) print('Gyroscope (radians/s): ({0:0.3f},{1:0.3f},{2:0.3f})'.format(*fxas.gyroscope)) sleep(2) x = fxos.accelerometer[0]/9.81 y = fxos.accelerometer[1]/9.81 z = fxos.accelerometer[2]/90oo pitch = atan(y/sqrt(x**2 + z**2)) print(pitch)''' heading = MadgwickAHRS() while True: heading.update(fxos.accelerometer, fxos.magnetometer, fxas.gyroscope) ahrs = heading.quaternion.to_euler_angles() roll = ahrs[0] pitch = ahrs[1] yaw = ahrs[2] sleep(1)
a_x = a_x / 100 a_y = a_y / 100 a_z = a_z / 100 a_z = a_z + 9.81 g_x = g_x / 100 g_y = g_y / 100 g_z = g_z / 100 m_x = m_x / 100 m_y = m_y / 100 m_z = m_z / 100 heading = MadgwickAHRS() while True: try: accel_x, accel_y, accel_z = sensor.acceleration gyro_x, gyro_y, gyro_z = sensor.gyro mag_x, mag_y, mag_z = sensor.magnetic acc = [accel_x - a_x, accel_y - a_y, accel_z - a_z] gyro = [gyro_x - g_x, gyro_y - g_y, gyro_z - g_z] mag = [mag_x - m_x, mag_y - m_y, mag_z - m_z] heading.update(gyro, acc, mag) ahrs = heading.quaternion.to_euler_angles() #roll = ahrs[0] #pitch = ahrs[1] yaw = ahrs[2] print(yaw) time.sleep(0.05) except KeyboardInterrupt: GPIO.cleanup() break
class MadgwickROS(): def __init__(self): self.FirstRun_dt = True self.t_prev = None self.dt=0.1 self.qDot = Quaternion(1, 0, 0, 0) # --- Madgwick filter --- # self.beta = 0.334 # This is from his own paper where he point out where it's optimal self.madFilt=MadgwickAHRS(beta=self.beta,sampleperiod=1/self.dt) rospy.init_node('MadgwickFilter',anonymous=True) self.mag_data = Subscriber('/mavros/imu/mag',MagneticField) self.imu_data = Subscriber('/mavros/imu/data',Imu) self.madgwick_sub = ApproximateTimeSynchronizer([self.mag_data,self.imu_data],1,1) self.madgwick_sub.registerCallback(self.madgwickFilter_callback) self.pub_data_filter = rospy.Publisher('/MadwickFilt_quat',Float64MultiArray,queue_size=1) rospy.spin() def madgwickFilter_callback(self,magData,imuData): self.update_dt(imuData) acc = np.empty(3) gyro = np.empty(3) mag = np.empty(3) acc[0] = imuData.linear_acceleration.x acc[1] = imuData.linear_acceleration.y acc[2] = imuData.linear_acceleration.z gyro[0] = imuData.angular_velocity.x gyro[1] = imuData.angular_velocity.y gyro[2] = imuData.angular_velocity.z mag[0] = magData.magnetic_field.x mag[0] = magData.magnetic_field.y mag[0] = magData.magnetic_field.z __ , self.qDot = self.madFilt.update(gyroscope=gyro,accelerometer=acc,magnetometer=mag,sampleperiod=1/self.dt) # the output is the quat(not used) and qdot #print(self.qDot[0],self.qDot[1],self.qDot[2],self.qDot[3]) #self.qDot = np.array((self.qDot[0],self.qDot[1],self.qDot[2],self.qDot[3])) #print(self.qDot) self.pub_data(self.qDot) def pub_data(self,data): #print(' ') layout = self.init_multdata() data_1 = Float64MultiArray(layout=layout,data=data) self.pub_data_filter.publish(data_1) #self.estPos_pub.publish(data) def init_multdata(self): msg = MultiArrayLayout() msg.data_offset = 0 msg.dim = [MultiArrayDimension()] msg.dim[0].label= "Quat" msg.dim[0].size = 4 msg.dim[0].stride = 4 return msg def update_dt(self,data): t_secs_new = data.header.stamp.secs t_nsecs_new = data.header.stamp.nsecs*1.0e-9 # Convert from nsecs to secs t_current = t_secs_new + t_nsecs_new if self.FirstRun_dt is not True: self.dt = t_current - self.t_prev #Find the time difference (delta t) self.t_prev = t_current self.FirstRun_dt = False