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))
