Commit e81344ae authored by andreadelprete's avatar andreadelprete
Browse files

Add test of madgwick

parent 5b56f260
# -*- 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
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment