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
