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 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)
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 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 __init__(self): self.Q_pos = np.eye(3) * 0.2 self.Q_vel = np.eye(3) * 1.0 self.Q_acc = np.eye(3) * 10.0 self.dataGPS = None self.dataIMU = None self.t_current = None self.t_prev = None self.FirstRun_dt = True self.GPS_on = False self.IMU_on = False self.qDot = Quaternion(1, 0, 0, 0) self.dt = 0.1 self.GPS_start = True self.GPS_0 = np.zeros([3, 1]) self.NED = np.zeros([3, 1]) self.rot_GPS = np.zeros([3, 3]) self.IMU_data = np.zeros([3, 1]) self.sensorData = np.zeros([9, 1]) ## ---- The Offset is calculated from previsly measurments --- # self.imu_x_offset = -0.1560196823083865 self.imu_y_offset = -0.12372256601510975 self.imu_z_offset = 9.8038682107820652 ## ---- Kalman filter setup ---- ## self.I = np.eye(3) self.ZERO = np.zeros((3, 3)) pv = self.I * self.dt pa = self.I * 0.5 * self.dt**2 P = np.hstack((self.I, pv, pa)) #P = np.hstack((self.I,pv,self.ZERO)) V = np.hstack((self.ZERO, self.I, pv)) a = np.hstack((self.ZERO, self.ZERO, self.I)) self.klmFilt = KalmanFilter(dim_x=9, dim_z=9) self.klmFilt.F = np.vstack((P, V, a)) C_gps = np.hstack((self.I, self.ZERO, self.ZERO)) C_vel = np.hstack((self.ZERO, self.ZERO, self.ZERO)) C_acc = np.hstack((self.ZERO, self.ZERO, self.I)) self.klmFilt.H = np.vstack((C_gps, C_vel, C_acc)) self.H_GPS = np.vstack((C_gps, C_vel, C_vel)) self.H_acc = np.vstack((C_vel, C_vel, C_acc)) if static_Q is True: self.klmFilt.Q = np.eye(9) self.klmFilt.Q[0:3, 0:3] = self.Q_pos self.klmFilt.Q[3:6, 3:6] = self.Q_vel self.klmFilt.Q[6:9, 6:9] = self.Q_acc else: self.klmFilt.Q = Q_discrete_white_noise(dim=3, dt=self.dt, block_size=3, order_by_dim=False) self.klmFilt.P *= 1000.0 self.klmFilt.R = self.klmFilt.H self.klmFilt.inv = np.linalg.pinv #The inv method is changed # This is done because it will gives a "linalgError: singular matrix" # --- 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) #init the node and subscribe to the GPS adn IMU rospy.init_node('KF_pos_estimation', anonymous=True) rospy.Subscriber('/mavros/global_position/raw/fix', NavSatFix, self.gps_data_load) rospy.Subscriber('/mavros/imu/data_raw', Imu, self.imu_data_load) #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) # creating the publisher #self.estPos_pub = rospy.Publisher('/KF_pos_est',numpy_msg(Floats),queue_size=1) self.estPos_pub_multarry = rospy.Publisher('/KF_pos_est', Float64MultiArray, queue_size=1) #rospy.Rate(10) # 10Hz rospy.spin()
gyroYInterpolated = np.interp(gyroTimeStampInterp, gyroTimeStamp, gyroY) gyroZInterpolated = np.interp(gyroTimeStampInterp, gyroTimeStamp, gyroZ) magTimeStampInterp = np.linspace(gpsTimeStampInterp[0], gpsTimeStampInterp[len(gpsTimeStampInterp) - 1], samples) magXInterpolated = np.interp(magTimeStampInterp, magTimeStamp, magX) 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
# plt.plot(time, acc_magFilt) # plt.plot(time[:(tempo_parado)*Fs], acc_magFilt[:(tempo_parado)*Fs]) # Threshold detection if ('ensaio_25' in name): stationary_threshold = 0.1 else: stationary_threshold = 0.1 stationary = acc_magFilt < stationary_threshold # ------------------------------------------------------------------------- # Compute orientation quat = [[0] * 4] * len(time) AHRSalgorithm = MadgwickAHRS( sampleperiod=np.round(1 / Fs, decimals=4)) # Initial convergence initPeriod = tempo_parado # usually 2 seconds # indexSel = 1 : find(sign(time-(time(1)+initPeriod))+1, 1); np.nonzero((np.sign(time - startTime) + 1) > 0)[0][0] indexSel = np.arange( 0, np.nonzero(np.sign(time - (time[0] + initPeriod)) + 1)[0][0], 1) for i in range(1, 2000): AHRSalgorithm.update_imu_new([0, 0, 0], [ accX[indexSel].mean(), accY[indexSel].mean(), accZ[indexSel].mean()
import smbus import time as t import numpy as np from madgwickahrs import MadgwickAHRS I2C_IMU_ADDRESS = 0x69 bus = smbus.SMBus(2) filter = MadgwickAHRS() def get_decimal(ls, ms): high = read_data(ms) << 8 return np.int16((high | read_data(ls))) def read_data(num): a = bus.read_byte_data(I2C_IMU_ADDRESS, num) # print(a) return a def get_data(): elapsed_time = t.process_time() accel_x = get_decimal(0x2E, 0x2D) accel_y = get_decimal(0x30, 0x2F) accel_z = get_decimal(0x32, 0x31) gyro_x = get_decimal(0x34, 0x33) gyro_y = get_decimal(0x36, 0x35)
angle = angle + delta_yaw else: if abs(delta_yaw) > dead_zone: angle = angle + delta_yaw print('Turn %f degeres heading %f degrees elapsed time:%f' % (delta_yaw, angle % 360, elapsed_time)) dt_vision.update_vision(76, 60, angle) angle = 90 dt_vision = SpoolMobileVision(59, 76, 60, angle, 1 * scale, 1 * scale, scale, canvas=w) # hedge = MarvelmindHedge(adr=59, tty="/dev/ttyACM0", baud=9600, maxvaluescount=32, recieveImuDataCallback=updateMiniatureWarehouseReferenceFrameData, debug=False) hedge = MarvelmindHedge( adr=59, tty="/dev/ttyACM0", baud=38400, maxvaluescount=3, recieveImuRawDataCallback=updateImuMadgwickFilteredData, debug=False) heading = MadgwickAHRS(sampleperiod=1 / 20, beta=2) hedge.start() mainloop()
mx_values_h = np.append(mx_values_h, mag_x_h) my_values_h = np.append(my_values_h, mag_y_h) mz_values_h = np.append(mz_values_h, mag_z_h) try: 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])
def newUpdateStateFunction(self, data): rotationData = data['rot_vector']['data'] linAccelerationData = data['lin_accel']['data'] accelerometerData = data['accel']['data'] magData = data['mag']['data'] gyroData = data['gyro']['data'] #print(linAccelerationData) for index, i in enumerate(rotationData, start=0): self.currentTime = i[0] timeDifference = (self.currentTime - self.previousTime) / 1000 self.currentQuaternion = pyrr.quaternion.create( i[1][0], i[1][1], i[1][2], i[1][3]) if self.currentTime != self.previousTime and self.previousTime != 0: if index < len(gyroData) and index < len( accelerometerData) and index < len(magData): self.madgwickahrs.update(gyroData[index][1], accelerometerData[index][1], magData[index][1], timeDifference) elif index < len(gyroData) and index < len(accelerometerData): self.madgwickahrs.update_imu(gyroData[index][1], accelerometerData[index][1], timeDifference) else: return a = pyrr.matrix33.create_from_inverse_of_quaternion( self.currentQuaternion) b = pyrr.matrix33.create_from_quaternion( self.initialQuaternion) rotationMatrix = a + b adjustedAccelerations = pyrr.matrix33.apply_to_vector( rotationMatrix, linAccelerationData[index][1]) # for i in range(0,len(adjustedAccelerations)): # if abs(adjustedAccelerations[i]) < 0.01: # adjustedAccelerations[i]=0 #adjustedVelocities= numpy.add(self.currentVelocities,numpy.add(adjustedAccelerations * timeDifference, numpy.array(numpy.add(adjustedAccelerations,self.currentAccelerations*-1)*timeDifference *timeDifference * 0.5))) #print("quaternion from madgwick"+str(self.madgwickahrs.quaternion._get_q())) #print("quaternion measured"+str(self.currentQuaternion)) interm1 = numpy.add( adjustedAccelerations, numpy.negative(self.currentAccelerations)) / timeDifference interm2 = interm1 * timeDifference * timeDifference * 0.5 interm3 = adjustedAccelerations * timeDifference interm4 = numpy.add(interm2, interm3) adjustedVelocities = numpy.add(self.currentVelocities, interm4) # for i in range(0,len(adjustedVelocities)): # if abs(adjustedVelocities[i]) < 0.01: # adjustedVelocities[i]=0 self.positionVectors = numpy.add( self.positionVectors, numpy.array(adjustedVelocities * timeDifference)) self.previousTime = self.currentTime self.currentAccelerations = adjustedAccelerations self.currentVelocities = adjustedVelocities #print(str(timeDifference)+ " s") #print("Adjusted Acceleration"+ str(adjustedAccelerations)) #print("unadjusted acceleration"+ str(data['accel']['data'][index][1])) #print("rotation vectors"+str(self.currentQuaternion)) #print("madgwick vectors"+str(self.madgwickahrs.quaternion._get_q())) #print("Adjusted Velocities"+ str(adjustedVelocities)) #print("Distance " + str(self.positionVectors)) print( "euler", Quaternion(self.currentQuaternion).to_euler123()[0] * 180 / math.pi) print() elif self.previousTime == 0: self.previousTime = self.currentTime self.currentAccelerations = linAccelerationData[index][1] #self.madgwickahrs=MadgwickAHRS(quaternion=Quaternion(i[1][0],i[1][1],i[1][2],i[1][3])) self.madgwickahrs = MadgwickAHRS( quaternion=Quaternion(self.currentQuaternion)) self.initialQuaternion = self.currentQuaternion
BUFSIZ = 128 ADDR = (HOST, PORT) imu = TroykaIMU() calibration_matrix = [[0.983175, 0.022738, -0.018581], [0.022738, 0.942140, -0.022467], [-0.018581, -0.022467, 1.016113]] # raw measurements only bias = [962.391696, -162.681348, 11832.188828] imu.magnetometer.calibrate_matrix(calibration_matrix, bias) imufilter = MadgwickAHRS(beta=1, sampleperiod=1/50) # Запрет на ожидание # tcpSerSock.setblocking(False) def print_log(text): print('{}\t{}'.format(datetime.datetime.now(), text)) def main(): while True: try: print_log('IMU Demo data server was started') print_log('waiting for connection...') # Ждем соединения клиента tcpCliSock, addr = tcpSerSock.accept() # Время ожидания данных от клиента
m_y += mag_y m_z += mag_z time.sleep(0.05) 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)
import utime from uPySensors.imu import MPU6050 from servo import Servo from machine import Pin import math import umatrix from madgwickahrs import MadgwickAHRS sensor = MPU6050(0) sensor.accel_range = 0 sensor.gyro_range = 1 sensor.filter_range = 1 ahrs = MadgwickAHRS(sampleperiod=0.150, beta=0.05) # Servo specific constants PULSE_MIN = 1.0 # in µs PULSE_MAX = 2.0 # in µs FREQUENCY = 50 # Hz ROTATIONAL_RANGE_100 = math.pi PIN0 = Pin.exp_board.G24 PIN1 = Pin.exp_board.G14 servo0 = Servo(PIN0, 0, FREQUENCY, ROTATIONAL_RANGE_100, PULSE_MIN, PULSE_MAX) servo1 = Servo(PIN1, 1, FREQUENCY, ROTATIONAL_RANGE_100, PULSE_MIN, PULSE_MAX) acc = [] gyro = [] while True:
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
gyro_x = read_gyro_x(address) gyro_y = read_gyro_y(address) 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)
#for i in range(1,len(time)-1): #predict from past state using rotation and acceleration in body axis # estimator.predict(i,rotations,acceleration) # s_priors = ukf.predict(i,rotations,acceleration) #s_priors6 = ukf6.predict(i,acceleration,omega) # measurement = estimator.meas_available(time[i],lh_traj) # estimator.update(measurement,i) # measurement = ukf.meas_available_azel(time[i],lighthouse_df, np.linalg.inv(R_eo) @ lh1, np.linalg.inv(R_eo) @ lh2) # ukf.update(measurement,i,s_priors) #ukf6.update(measurement,i,s_priors6) start = 2500 comp_filt = MadgwickAHRS(1 / 60, quaternion=None, beta=1) comp_eulers = [] ukf6 = UKF6DoF(Q=Q_6, R=R_ukf, 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,
if comptime > 5: gcomp = gcomp * GYRO_FACTOR / comptime print("Gcomp:", gcomp) break; # ------------------------------- # 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:
(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)
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)
imu_df['timestamp_optitrack'].iloc[start], imu_df['timestamp_optitrack'].iloc[len(imu_df) - 1], 1 / 100) estimator = Kalman(Q=Q, R=R, xm_0=xm_0, Pm_0=Pm_0, time=imu_df['timestamp_optitrack'], R_eo=np.eye(3), thresh=100, t0=imu_df['timestamp_optitrack'].iloc[start]) #setup inital rotation for madgwick filter initial_rot = Rotation(1, 0, 0, 0).from_euler_YPR([0, .3, 2.8]) initial_quat = Quaternion(initial_rot.q[0], initial_rot.q[1], initial_rot.q[2], initial_rot.q[3]) comp_filt = MadgwickAHRS(1 / 60, quaternion=initial_quat, beta=1) comp_eulers = [] rot_last = None #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):
import rospy import tf2_ros import madgwickahrs from madgwickahrs import MadgwickAHRS import quaternion from quaternion import Quaternion import numpy as np from sensor_msgs.msg import Imu from geometry_msgs.msg import QuaternionStamped from geometry_msgs.msg import TransformStamped mad = MadgwickAHRS( sampleperiod=.001, # 1000 DPS quaternion=Quaternion(1, 0, 0, 0), beta=1) """ Save data to CSV """ def save_data(gyro, accel, mag): with open('gyro_data.csv', 'a+') as gyro_file: gyro_csv_str = ','.join(['%.5f' % num for num in gyro]) gyro_file.write(gyro_csv_str + '\n') with open('accel_data.csv', 'a+') as accel_file: accel_csv_str = ','.join(['%.5f' % num for num in accel]) accel_file.write(accel_csv_str + '\n') with open('mag_data.csv', 'a+') as mag_file: mag_csv_str = ','.join(['%.5f' % num for num in mag])
plt.grid() plt.plot(time, magX, 'r') plt.plot(time, magY, 'g') plt.plot(time, magZ, 'b') plt.title('Magnetrometer') plt.ylabel('Magnetic Flux Density (G)') plt.legend(['X', 'Y', 'Z']) 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:
def __init__(self): self.sense = SenseHat() self.madgwick = MadgwickAHRS(.055, None, 1)
def newUpdateStateFunction(self, data): if self.up == True: rotationData = data['rot_vector']['data'] linAccelerationData = data['lin_accel']['data'] accelerometerData = data['accel']['data'] magData = data['mag']['data'] gyroData = data['gyro']['data'] #print(linAccelerationData) for index, i in enumerate(rotationData, start=0): self.currentTime = i[0] if self.up == False: break timeDifference = (self.currentTime - self.previousTime) / 1000 self.currentQuaternion = pyrr.quaternion.create( i[1][0], i[1][1], i[1][2], i[1][3]) quat = self.currentQuaternion if self.currentTime != self.previousTime and self.previousTime != 0: if index < len(gyroData) and index < len( accelerometerData) and index < len(magData): self.madgwickahrs.update(gyroData[index][1], accelerometerData[index][1], magData[index][1], timeDifference) #elif index < len(gyroData) and index < len(accelerometerData): #self.madgwickahrs.update_imu(gyroData[index][1],accelerometerData[index][1] ,timeDifference) #self.currentQuaternion=pyrr.Quaternion.from_eulers(self.madgwickahrs.quaternion.to_euler123()) #c= self.currentQuaternion-self.initialQuaternion a = pyrr.matrix33.create_from_inverse_of_quaternion( self.currentQuaternion) b = pyrr.matrix33.create_from_quaternion( self.initialQuaternion) adjustedAccelerations1 = pyrr.matrix33.apply_to_vector( a, linAccelerationData[index][1]) adjustedAccelerations = pyrr.matrix33.apply_to_vector( b, adjustedAccelerations1) #adjustedAccelerations=adjustedAccelerations- numpy.array([ 1.14548891e-05 , 3.24514926e-05, -3.21555242e-04]) #filtered=self.highpass(adjustedAccelerations) #for i in range(0,len(adjustedAccelerations)): # if abs(adjustedAccelerations[i]) < 0.01: # adjustedAccelerations[i]=0 kalmanx = self.kalmanX.update(adjustedAccelerations[0]) kalmany = self.kalmanY.update(adjustedAccelerations[1]) kalmanz = self.kalmanZ.update(adjustedAccelerations[2]) print("x" + str(kalmanx)) print("y" + str(kalmany)) print("z" + str(kalmanz)) #adjustedAccelerations= [kalmanx[2],kalmany[2],kalmanz[2]] #adjustedAccelerations= self.lowPass(adjustedAccelerations) #for i in range(0,len(adjustedAccelerations)): # if abs(adjustedAccelerations[i]) < 0.01: # adjustedAccelerations[i]=0 #adjustedVelocities= numpy.add(self.currentVelocities,numpy.add(adjustedAccelerations * timeDifference, numpy.array(numpy.add(adjustedAccelerations,self.currentAccelerations*-1)*timeDifference *timeDifference * 0.5))) #adjustedVelocities=[kalmanx[1],kalmany[1],kalmanz[1]] #adjustedAccelerations=numpy.array([self.kalmanX.update(adjustedAccelerations[0]),self.kalmanY.update(adjustedAccelerations[1]),self.kalmanZ.update(adjustedAccelerations[2])]) #print("quaternion from madgwick"+str(self.madgwickahrs.quaternion._get_q())) #print("quaternion measured"+str(self.currentQuaternion)) for i in range(0, len(adjustedAccelerations)): if abs(adjustedAccelerations[i]) < 0.02: adjustedAccelerations[i] = 0 #self.average= (self.average * self.num + adjustedAccelerations)/( self.num + 1) #self.num+=1 interm1 = numpy.add( adjustedAccelerations, numpy.negative( self.currentAccelerations)) / timeDifference interm2 = numpy.array( interm1) * timeDifference * timeDifference * 0.5 #interm2=0 interm3 = numpy.array( self.currentAccelerations) * timeDifference interm4 = numpy.add(interm2, interm3) adjustedVelocities = numpy.add(self.currentVelocities, interm4) for k in range(0, len(adjustedVelocities)): if abs(adjustedVelocities[k]) < 0.1: adjustedVelocities[k] = 0.0 interm5 = 0.5 * interm3 * timeDifference interm6 = (interm2 * timeDifference) / 3 #self.positionVectors=numpy.add(self.positionVectors,interm5) #self.positionVectors=numpy.add(self.positionVectors,interm6) #adjustedVelocities=numpy.array([kalmanx[1],kalmany[1],kalmanz[1]]) self.positionVectors = numpy.add( self.positionVectors, numpy.array(adjustedVelocities * timeDifference)) self.previousTime = self.currentTime self.currentAccelerations = adjustedAccelerations self.currentVelocities = adjustedVelocities self.previousMeasurement = linAccelerationData[index][1] print(str(timeDifference) + " s") print(str(self.num)) #print("Average"+str(self.average)) print("Adjusted Acceleration" + str(adjustedAccelerations)) #print("Filtered Acceleration"+ str(filtered)) print("unadjusted acceleration" + str(data['lin_accel']['data'][index][1])) print("rotation vectors" + str(quat)) print("madgwick vectors" + str(self.madgwickahrs.quaternion.q)) print("Adjusted Velocities" + str(adjustedVelocities)) print("Distance " + str(self.positionVectors)) # for j in range(0,len(self.positionVectors)): # if abs(self.positionVectors[j])>1: # sleep(10) #print("euler", Quaternion(self.currentQuaternion).to_euler123()[0]*180/3.14) print("__________________________________________________") elif self.previousTime == 0: self.previousTime = self.currentTime #self.currentAccelerations=linAccelerationData[index][1] #self.madgwickahrs=MadgwickAHRS(quaternion=Quaternion(i[1][0],i[1][1],i[1][2],i[1][3])) self.madgwickahrs = MadgwickAHRS( quaternion=Quaternion(self.currentQuaternion)) self.initialQuaternion = self.currentQuaternion self.previousMeasurement = linAccelerationData[index][1] self.kalmanX = Kalman_Accel(0) self.kalmanY = Kalman_Accel(0) self.kalmanZ = Kalman_Accel(0) else: self.currentAccelerations = numpy.zeros(3) self.currentVelocities = numpy.zeros(3) self.previousAccelerations = numpy.zeros(3) print("Not Moving") print("Distance " + str(self.positionVectors)) if self.previousTime: print("rotation vectors" + str(self.currentQuaternion))