<commit_before>
#quaternion initialization and beta
q0 = 1.0
q1 = 0.0
q2 = 0.0
q3 = 0.0
beta = 0.1

function invsqrt(x)
	return 1/sqrt(x)
end

function AHRS_update_IMU( gx,  gy,  gz,  ax,  ay,  az, q0, q1, q2, q3)


	#quaternion rate change
	qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz)
	qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy)
	qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx)
	qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx)

	#Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if !((ax == 0.0) && (ay == 0.0) && (az == 0.0)) 

		# Normalise accelerometer measurement
		recipNorm = invsqrt(ax * ax + ay * ay + az * az)
		ax = ax * recipNorm
		ay = ay * recipNorm
		az = az * recipNorm  

		# Auxiliary variables to avoid repeated arithmetic
		two_q0 = 2.0 * q0
		two_q1 = 2.0 * q1
		two_q2 = 2.0 * q2
		two_q3 = 2.0 * q3
		four_q0 = 4.0 * q0
		four_q1 = 4.0 * q1
		four_q2 = 4.0 * q2
		eight_q1 = 8.0 * q1
		eight_q2 = 8.0 * q2
		q0q0 = q0 * q0
		q1q1 = q1 * q1
		q2q2 = q2 * q2
		q3q3 = q3 * q3

		# Gradient decent algorithm corrective step
		s0 = four_q0 * q2q2 + two_q2 * ax + four_q0 * q1q1 - two_q1 * ay
		s1 = four_q1 * q3q3 - two_q3 * ax + 4.0 * q0q0 * q1 - two_q0 * ay - four_q1 + eight_q1 * q1q1 + eight_q1 * q2q2 + four_q1 * az
		s2 = 4.0 * q0q0 * q2 + two_q0 * ax + four_q2 * q3q3 - two_q3 * ay - four_q2 + eight_q2 * q1q1 + eight_q2 * q2q2 + four_q2 * az
		s3 = 4.0 * q1q1 * q3 - two_q1 * ax + 4.0 * q2q2 * q3 - two_q2 * ay
		recipNorm = invsqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3) # normalise step magnitude
		print(s0," ", s1," ", s2," ", s3, " ", recipNorm, " \n")
		s0 = s0 * recipNorm
		s1 = s1 * recipNorm
		s2 = s2 * recipNorm
		s3 = s3 * recipNorm

		# Apply feedback step
		qDot1 = qDot1 - beta * s0
		qDot2 = qDot2 - beta * s1
		qDot3 = qDot3 - beta * s2
		qDot4 = qDot4 - beta * s3
	end
	

	# Integrate rate of change of quaternion to yield quaternion

	#print(recipNorm ,"\n")
	#print(s0," ", s1," ", s2," ", s3, " \n")
	#print(qDot1," ", qDot2," ", qDot3," ", qDot4, " \n")
	q0 = q0 +  qDot1 * (1.0 / 512.0)
	q1 = q1 +  qDot2 * (1.0 / 512.0)
	q2 = q2 +  qDot3 * (1.0 / 512.0)
	q3 = q3 +  qDot4 * (1.0 / 512.0)

	# Normalise quaternion
	recipNorm = invsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
	q0 = q0 * recipNorm
	q1 = q1 * recipNorm
	q2 = q2 * recipNorm
	q3 = q3 * recipNorm
	return q0, q1, q2, q3

end

function AHRS_update( gx,  gy,  gz,  ax,  ay,  az,  mx,  my,  mz, q0, q1, q2, q3)
	# Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
	if (mx == 0.0) && (my == 0.0) && (mz == 0.0) 
		q0, q1, q2, q3 = AHRS_update_IMU(gx, gy, gz, ax, ay, az, q0, q1, q2, q3)
		return q0, q1, q2, q3
	end
	

	# Rate of change of quaternion from gyroscope
	qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz)
	qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy)
	qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx)
	qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx)

	# Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if !((ax == 0.0) && (ay == 0.0) && (az == 0.0)) 

		# Normalise accelerometer measurement
		recipNorm = invsqrt(ax * ax + ay * ay + az * az)
		ax = ax * recipNorm
		ay = ay * recipNorm
		az = az * recipNorm  

		# Normalise magnetometer measurement
		recipNorm = invsqrt(mx * mx + my * my + mz * mz)
		mx = mx * recipNorm
		my = my * recipNorm
		mz = mz * recipNorm

		# Auxiliary variables to avoid repeated arithmetic
		two_q0mx = 2.0 * q0 * mx
		two_q0my = 2.0 * q0 * my
		two_q0mz = 2.0 * q0 * mz
		two_q1mx = 2.0 * q1 * mx
		two_q0 = 2.0 * q0
		two_q1 = 2.0 * q1
		two_q2 = 2.0 * q2
		two_q3 = 2.0 * q3
		two_q0q2 = 2.0 * q0 * q2
		two_q2q3 = 2.0 * q2 * q3
		q0q0 = q0 * q0
		q0q1 = q0 * q1
		q0q2 = q0 * q2
		q0q3 = q0 * q3
		q1q1 = q1 * q1
		q1q2 = q1 * q2
		q1q3 = q1 * q3
		q2q2 = q2 * q2
		q2q3 = q2 * q3
		q3q3 = q3 * q3

		# Reference direction of Earth's magnetic field
		hx = mx * q0q0 - two_q0my * q3 + two_q0mz * q2 + mx * q1q1 + two_q1 * my * q2 + two_q1 * mz * q3 - mx * q2q2 - mx * q3q3
		hy = two_q0mx * q3 + my * q0q0 - two_q0mz * q1 + two_q1mx * q2 - my * q1q1 + my * q2q2 + two_q2 * mz * q3 - my * q3q3
		two_bx = sqrt(hx * hx + hy * hy)
		two_bz = -two_q0mx * q2 + two_q0my * q1 + mz * q0q0 + two_q1mx * q3 - mz * q1q1 + two_q2 * my * q3 - mz * q2q2 + mz * q3q3
		four_bx = 2.0 * two_bx
		four_bz = 2.0 * two_bz

		# Gradient decent algorithm corrective step
		s0 = -two_q2 * (2.0 * q1q3 - two_q0q2 - ax) + two_q1 * (2.0 * q0q1 + two_q2q3 - ay) - two_bz * q2 * (two_bx * (0.5 - q2q2 - q3q3) + two_bz * (q1q3 - q0q2) - mx) + (-two_bx * q3 + two_bz * q1) * (two_bx * (q1q2 - q0q3) + two_bz * (q0q1 + q2q3) - my) + two_bx * q2 * (two_bx * (q0q2 + q1q3) + two_bz * (0.5 - q1q1 - q2q2) - mz)
		s1 = two_q3 * (2.0 * q1q3 - two_q0q2 - ax) + two_q0 * (2.0 * q0q1 + two_q2q3 - ay) - 4.0 * q1 * (1 - 2.0 * q1q1 - 2.0 * q2q2 - az) + two_bz * q3 * (two_bx * (0.5 - q2q2 - q3q3) + two_bz * (q1q3 - q0q2) - mx) + (two_bx * q2 + two_bz * q0) * (two_bx * (q1q2 - q0q3) + two_bz * (q0q1 + q2q3) - my) + (two_bx * q3 - four_bz * q1) * (two_bx * (q0q2 + q1q3) + two_bz * (0.5 - q1q1 - q2q2) - mz)
		s2 = -two_q0 * (2.0 * q1q3 - two_q0q2 - ax) + two_q3 * (2.0 * q0q1 + two_q2q3 - ay) - 4.0 * q2 * (1 - 2.0 * q1q1 - 2.0 * q2q2 - az) + (-four_bx * q2 - two_bz * q0) * (two_bx * (0.5 - q2q2 - q3q3) + two_bz * (q1q3 - q0q2) - mx) + (two_bx * q1 + two_bz * q3) * (two_bx * (q1q2 - q0q3) + two_bz * (q0q1 + q2q3) - my) + (two_bx * q0 - four_bz * q2) * (two_bx * (q0q2 + q1q3) + two_bz * (0.5 - q1q1 - q2q2) - mz)
		s3 = two_q1 * (2.0 * q1q3 - two_q0q2 - ax) + two_q2 * (2.0 * q0q1 + two_q2q3 - ay) + (-four_bx * q3 + two_bz * q1) * (two_bx * (0.5 - q2q2 - q3q3) + two_bz * (q1q3 - q0q2) - mx) + (-two_bx * q0 + two_bz * q2) * (two_bx * (q1q2 - q0q3) + two_bz * (q0q1 + q2q3) - my) + two_bx * q1 * (two_bx * (q0q2 + q1q3) + two_bz * (0.5 - q1q1 - q2q2) - mz)
		recipNorm = invsqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3) # normalise step magnitude
		s0 = s0 * recipNorm
		s1 = s1 * recipNorm
		s2 = s2 * recipNorm
		s3 = s3 * recipNorm

		# Apply feedback step
		qDot1 = qDot1 - beta * s0
		qDot2 = qDot2 - beta * s1
		qDot3 = qDot3 - beta * s2
		qDot4 = qDot4 - beta * s3
	end
	
	# Integrate rate of change of quaternion to yield quaternion
	q0 = q0 + qDot1 * (1.0 / 512.0)
	q1 = q1 + qDot2 * (1.0 / 512.0)
	q2 = q2 + qDot3 * (1.0 / 512.0)
	q3 = q3 + qDot4 * (1.0 / 512.0)

	# Normalise quaternion
	recipNorm = invsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
	q0 = q0 * recipNorm
	q1 = q1 * recipNorm
	q2 = q2 * recipNorm
	q3 = q3 * recipNorm
	return q0, q1, q2, q3
end



gx = 30.0
gy = 30.0
gz = 30.0
ax = 1.0
ay = 1.0
az = 1.0
mx = 30.0
my = 30.0<commit_msg>Some corrections, now working prioperly comments portugues<commit_after>
#quaternion initialization and beta
q0 = 1.0
q1 = 0.0
q2 = 0.0
q3 = 0.0
beta = 0.1

function invsqrt(x)
	return 1/sqrt(x)
end

function AHRS_update_IMU( gx,  gy,  gz,  ax,  ay,  az, q0, q1, q2, q3)


	gx = gx * 0.0174533;
	gy = gy * 0.0174533;
	gz = gz * 0.0174533;

	#quaternion rate change
	qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz)
	qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy)
	qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx)
	qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx)

	#Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if !((ax == 0.0) && (ay == 0.0) && (az == 0.0)) 

		# Normalise accelerometer measurement
		norm = invsqrt(ax * ax + ay * ay + az * az)
		ax = ax * norm
		ay = ay * norm
		az = az * norm  

		# Auxiliary variables to avoid repeated arithmetic
		two_q0 = 2.0 * q0
		two_q1 = 2.0 * q1
		two_q2 = 2.0 * q2
		two_q3 = 2.0 * q3
		four_q0 = 4.0 * q0
		four_q1 = 4.0 * q1
		four_q2 = 4.0 * q2
		eight_q1 = 8.0 * q1
		eight_q2 = 8.0 * q2
		q0q0 = q0 * q0
		q1q1 = q1 * q1
		q2q2 = q2 * q2
		q3q3 = q3 * q3

		# Gradient decent algorithm corrective step
		s0 = four_q0 * q2q2 + two_q2 * ax + four_q0 * q1q1 - two_q1 * ay
		s1 = four_q1 * q3q3 - two_q3 * ax + 4.0 * q0q0 * q1 - two_q0 * ay - four_q1 + eight_q1 * q1q1 + eight_q1 * q2q2 + four_q1 * az
		s2 = 4.0 * q0q0 * q2 + two_q0 * ax + four_q2 * q3q3 - two_q3 * ay - four_q2 + eight_q2 * q1q1 + eight_q2 * q2q2 + four_q2 * az
		s3 = 4.0 * q1q1 * q3 - two_q1 * ax + 4.0 * q2q2 * q3 - two_q2 * ay
		norm = invsqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3) # normalise step magnitude
		print(s0," ", s1," ", s2," ", s3, " ", norm, " \n")
		s0 = s0 * norm
		s1 = s1 * norm
		s2 = s2 * norm
		s3 = s3 * norm

		# Apply feedback step
		qDot1 = qDot1 - beta * s0
		qDot2 = qDot2 - beta * s1
		qDot3 = qDot3 - beta * s2
		qDot4 = qDot4 - beta * s3
	end
	

	# Integrate rate of change of quaternion to yield quaternion

	#print(norm ,"\n")
	#print(s0," ", s1," ", s2," ", s3, " \n")
	#print(qDot1," ", qDot2," ", qDot3," ", qDot4, " \n")
	q0 = q0 +  qDot1 * (1.0 / 512.0)
	q1 = q1 +  qDot2 * (1.0 / 512.0)
	q2 = q2 +  qDot3 * (1.0 / 512.0)
	q3 = q3 +  qDot4 * (1.0 / 512.0)

	# Normalise quaternion
	norm = invsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
	q0 = q0 * norm
	q1 = q1 * norm
	q2 = q2 * norm
	q3 = q3 * norm
	return q0, q1, q2, q3

end

function AHRS_update( gx,  gy,  gz,  ax,  ay,  az,  mx,  my,  mz, q0, q1, q2, q3)
	# Usa IMU para o caso se a medição do magnetometro ser invalida
	if (mx == 0.0) && (my == 0.0) && (mz == 0.0) 
		q0, q1, q2, q3 = AHRS_update_IMU(gx, gy, gz, ax, ay, az, q0, q1, q2, q3)
		return q0, q1, q2, q3
	end
	# De graus/sec pra rad/sec
	gx = gx * 0.0174533;
	gy = gy * 0.0174533;
	gz = gz * 0.0174533;

	# Taxa de variação do quaternion pelo giroscopio
	qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz)
	qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy)
	qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx)
	qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx)

	if !((ax == 0.0) && (ay == 0.0) && (az == 0.0)) 

		# Normalizando dados do acelerometro
		norm = invsqrt(ax * ax + ay * ay + az * az)
		ax = ax * norm
		ay = ay * norm
		az = az * norm  

		# Normaliza magnetometro
		norm = invsqrt(mx * mx + my * my + mz * mz)
		mx = mx * norm
		my = my * norm
		mz = mz * norm

		# Pra nao repetir calculos
		two_q0mx = 2.0 * q0 * mx
		two_q0my = 2.0 * q0 * my
		two_q0mz = 2.0 * q0 * mz
		two_q1mx = 2.0 * q1 * mx
		two_q0 = 2.0 * q0
		two_q1 = 2.0 * q1
		two_q2 = 2.0 * q2
		two_q3 = 2.0 * q3
		two_q0q2 = 2.0 * q0 * q2
		two_q2q3 = 2.0 * q2 * q3
		q0q0 = q0 * q0
		q0q1 = q0 * q1
		q0q2 = q0 * q2
		q0q3 = q0 * q3
		q1q1 = q1 * q1
		q1q2 = q1 * q2
		q1q3 = q1 * q3
		q2q2 = q2 * q2
		q2q3 = q2 * q3
		q3q3 = q3 * q3

		# compensação da direção do campo magnetico
		hx = mx * q0q0 - two_q0my * q3 + two_q0mz * q2 + mx * q1q1 + two_q1 * my * q2 + two_q1 * mz * q3 - mx * q2q2 - mx * q3q3
		hy = two_q0mx * q3 + my * q0q0 - two_q0mz * q1 + two_q1mx * q2 - my * q1q1 + my * q2q2 + two_q2 * mz * q3 - my * q3q3
		two_bx = sqrt(hx * hx + hy * hy)
		two_bz = -two_q0mx * q2 + two_q0my * q1 + mz * q0q0 + two_q1mx * q3 - mz * q1q1 + two_q2 * my * q3 - mz * q2q2 + mz * q3q3
		four_bx = 2.0 * two_bx
		four_bz = 2.0 * two_bz

		# Gradiente descendente
		s0 = -two_q2 * (2.0 * q1q3 - two_q0q2 - ax) + two_q1 * (2.0 * q0q1 + two_q2q3 - ay) - two_bz * q2 * (two_bx * (0.5 - q2q2 - q3q3) + two_bz * (q1q3 - q0q2) - mx) + (-two_bx * q3 + two_bz * q1) * (two_bx * (q1q2 - q0q3) + two_bz * (q0q1 + q2q3) - my) + two_bx * q2 * (two_bx * (q0q2 + q1q3) + two_bz * (0.5 - q1q1 - q2q2) - mz)
		s1 = two_q3 * (2.0 * q1q3 - two_q0q2 - ax) + two_q0 * (2.0 * q0q1 + two_q2q3 - ay) - 4.0 * q1 * (1 - 2.0 * q1q1 - 2.0 * q2q2 - az) + two_bz * q3 * (two_bx * (0.5 - q2q2 - q3q3) + two_bz * (q1q3 - q0q2) - mx) + (two_bx * q2 + two_bz * q0) * (two_bx * (q1q2 - q0q3) + two_bz * (q0q1 + q2q3) - my) + (two_bx * q3 - four_bz * q1) * (two_bx * (q0q2 + q1q3) + two_bz * (0.5 - q1q1 - q2q2) - mz)
		s2 = -two_q0 * (2.0 * q1q3 - two_q0q2 - ax) + two_q3 * (2.0 * q0q1 + two_q2q3 - ay) - 4.0 * q2 * (1 - 2.0 * q1q1 - 2.0 * q2q2 - az) + (-four_bx * q2 - two_bz * q0) * (two_bx * (0.5 - q2q2 - q3q3) + two_bz * (q1q3 - q0q2) - mx) + (two_bx * q1 + two_bz * q3) * (two_bx * (q1q2 - q0q3) + two_bz * (q0q1 + q2q3) - my) + (two_bx * q0 - four_bz * q2) * (two_bx * (q0q2 + q1q3) + two_bz * (0.5 - q1q1 - q2q2) - mz)
		s3 = two_q1 * (2.0 * q1q3 - two_q0q2 - ax) + two_q2 * (2.0 * q0q1 + two_q2q3 - ay) + (-four_bx * q3 + two_bz * q1) * (two_bx * (0.5 - q2q2 - q3q3) + two_bz * (q1q3 - q0q2) - mx) + (-two_bx * q0 + two_bz * q2) * (two_bx * (q1q2 - q0q3) + two_bz * (q0q1 + q2q3) - my) + two_bx * q1 * (two_bx * (q0q2 + q1q3) + two_bz * (0.5 - q1q1 - q2q2) - mz)
		norm = invsqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3) # normalise step magnitude
		s0 = s0 * norm
		s1 = s1 * norm
		s2 = s2 * norm
		s3 = s3 * norm

		# passo do feedback
		qDot1 = qDot1 - beta * s0
		qDot2 = qDot2 - beta * s1
		qDot3 = qDot3 - beta * s2
		qDot4 = qDot4 - beta * s3
	end
	
	# aplicando no quaterinon
	q0 = q0 + qDot1 * (1.0 / 512.0)
	q1 = q1 + qDot2 * (1.0 / 512.0)
	q2 = q2 + qDot3 * (1.0 / 512.0)
	q3 = q3 + qDot4 * (1.0 / 512.0)

	# Normalizando
	norm = invsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
	q0 = q0 * norm
	q1 = q1 * norm
	q2 = q2 * norm
	q3 = q3 * norm
	return q0, q1, q2, q3
end



gx = 30.0
gy = 30.0
gz = 30.0
ax = 1.0
ay = 1.0
az = 1.0
mx = 30.0
my = 30.0
{'fraction_non_alphanumeric': 0.16202723146747353, 'fraction_numerical': 0.11951588502269289, 'mean_word_length': 2.165948275862069, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '457607', 'n_tokens_mistral': 7593, 'n_tokens_neox': 7029, 'n_words': 1879}