from ahrs.common.orientation import triad
from ahrs.common.quaternion import sarabandi
if __name__ == "__main__":
from ahrs.utils import plot
data = np.genfromtxt('../../tests/repoIMU.csv',
dtype=float,
delimiter=';',
skip_header=2)
q_ref = data[:, 1:5]
acc = data[:, 5:8]
mag = data[:, 11:14]
num_samples = data.shape[0]
# Estimate Orientations with IMU
q = np.tile([1., 0., 0., 0.], (num_samples, 1))
for i in range(num_samples):
dcm = triad(acc[i], mag[i])
q[i] = sarabandi(dcm)
# Compute Error
sqe = abs(q_ref - q).sum(axis=1)**2
# Plot results
plot(q_ref,
q,
sqe,
title="TRIAD estimation",
subtitles=[
"Reference Quaternions", "Estimated Quaternions", "Squared Errors"
],
yscales=["linear", "linear", "log"],
labels=[[], [], ["MSE = {:.3e}".format(sqe.mean())]])
# Error is sum of cross product between estimated direction and measured direction of fields
e = np.cross(a, v) + np.cross(m, 2.0 * w)
self.eInt = self.eInt + e * self.samplePeriod if self.Ki > 0 else np.array(
[0.0, 0.0, 0.0])
# Apply feedback term
g += self.Kp * e + self.Ki * self.eInt
# Compute rate of change of quaternion
qDot = 0.5 * q_prod(q, [0.0, g[0], g[1], g[2]])
# Integrate to yield Quaternion
q += qDot * self.samplePeriod
q /= np.linalg.norm(q)
return q
if __name__ == '__main__':
from ahrs.utils import plot
data = np.genfromtxt('../../tests/repoIMU.csv',
dtype=float,
delimiter=';',
skip_header=2)
acc = data[:, 5:8]
gyr = data[:, 8:11]
mahony = Mahony()
num_samples = data.shape[0]
qts = np.tile([1., 0., 0., 0.], (num_samples, 1))
for i in range(1, num_samples):
qts[i] = mahony.updateIMU(qts[i - 1], gyr[i], acc[i])
plot(data[:, 1:5],
qts,
subtitles=["Reference Quaternions", "Estimated Quaternions"])
mag = data[:, 11:14]
num_samples = data.shape[0]
# Estimate Orientations with IMU
q_imu = np.tile([1., 0., 0., 0.], (num_samples, 1))
mahony = Mahony()
for i in range(1, num_samples):
q_imu[i] = mahony.updateIMU(q_imu[i - 1], gyr[i], acc[i])
# Estimate Orientations with MARG
q_marg = np.tile([1., 0., 0., 0.], (num_samples, 1))
mahony = Mahony()
for i in range(1, num_samples):
q_marg[i] = mahony.updateMARG(q_marg[i - 1], gyr[i], acc[i], mag[i])
# Compute Error
sqe_imu = abs(q_ref - q_imu).sum(axis=1)**2
sqe_marg = abs(q_ref - q_marg).sum(axis=1)**2
# Plot results
plot(data[:, 1:5],
q_imu,
q_marg, [sqe_imu, sqe_marg],
title="Mahony's algorithm",
subtitles=[
"Reference Quaternions", "Estimated Quaternions (IMU)",
"Estimated Quaternions (MARG)", "Squared Errors"
],
yscales=["linear", "linear", "linear", "log"],
labels=[[], [], [],
[
"MSE (IMU) = {:.3e}".format(sqe_imu.mean()),
"MSE (MARG) = {:.3e}".format(sqe_marg.mean())
]])