def create_imu_filter(robot, dt):
from dynamic_graph.sot.torque_control.madgwickahrs import MadgwickAHRS
imu_filter = MadgwickAHRS('imu_filter');
imu_filter.init(dt);
plug(robot.imu_offset_compensation.accelerometer_out, imu_filter.accelerometer);
plug(robot.imu_offset_compensation.gyrometer_out, imu_filter.gyroscope);
return imu_filter;
# -*- coding: utf-8 -*-
"""
Created on Tue Oct 3 14:01:08 2017
@author: adelpret
"""
from dynamic_graph.sot.torque_control.madgwickahrs import MadgwickAHRS
dt = 0.001
imu_filter = MadgwickAHRS('imu_filter')
imu_filter.init(dt)
imu_filter.setBeta(0.0)
for i in range(10):
imu_filter.accelerometer.value = (0.0, 0.0, 9.8)
imu_filter.gyroscope.value = (0.001, -1e-3, 1e-4)
imu_filter.imu_quat.recompute(i)
print(imu_filter.imu_quat.value)