%?Examplescript.m
%
%?This?script?demonstrates?use?of?the?MadgwickAHRS?and?MahonyAHRS?algorithm
%?classes?with?example?data.?ExampleData.mat?contains?calibrated?gyroscope
%?accelerometer?and?magnetometer?data?logged?from?an?AHRS?device?（x-IMU）
%?while?it?was?sequentially?rotated?from?0?degrees?to?+90?degree?and?then
%?to?-90?degrees?around?the?X?Y?and?Z?axis.??The?script?first?plots?the
%?example?sensor?data?then?processes?the?data?through?the?algorithm?and
%?plots?the?output?as?Euler?angles.
%
%?Note?that?the?Euler?angle?plot?shows?erratic?behaviour?in?phi?and?psi
%?when?theta?approaches??0?degrees.?This?due?to?a?singularity?in?the?Euler
%?angle?sequence?known?as?‘Gimbal?lock‘.??This?issue?does?not?exist?for?a
%?quaternion?or?rotation?matrix?representation.
%
%?Date??????????Author??????????Notes
%?28/09/2011????SOH?Madgwick????Initial?release
%?13/04/2012????SOH?Madgwick????deg2rad?function?no?longer?used
%?06/11/2012????Seb?Madgwick????radian?to?degrees?calculation?corrected

%%?Start?of?script

addpath（‘quaternion_library‘）;??????%?include?quaternion?library
close?all;??????????????????????????%?close?all?figures
clear;??????????????????????????????%?clear?all?variables
clc;????????????????????????????????%?clear?the?command?terminal

%%?Import?and?plot?sensor?data

load（‘ExampleData.mat‘）;

figure（‘Name‘?‘Sensor?Data‘）;
axis（1）?=?subplot（311）;
hold?on;
plot（time?Gyroscope（:1）?‘r‘）;
plot（time?Gyroscope（:2）?‘g‘）;
plot（time?Gyroscope（:3）?‘b‘）;
legend（‘X‘?‘Y‘?‘Z‘）;
xlabel（‘Time?（s）‘）;
ylabel（‘Angular?rate?（deg/s）‘）;
title（‘Gyroscope‘）;
hold?off;
axis（2）?=?subplot（312）;
hold?on;
plot（time?Accelerometer（:1）?‘r‘）;
plot（time?Accelerometer（:2）?‘g‘）;
plot（time?Accelerometer（:3）?‘b‘）;
legend（‘X‘?‘Y‘?‘Z‘）;
xlabel（‘Time?（s）‘）;
ylabel（‘Acceleration?（g）‘）;
title（‘Accelerometer‘）;
hold?off;
axis（3）?=?subplot（313）;
hold?on;
plot（time?Magnetometer（:1）?‘r‘）;
plot（time?Magnetometer（:2）?‘g‘）;
plot（time?Magnetometer（:3）?‘b‘）;
legend（‘X‘?‘Y‘?‘Z‘）;
xlabel（‘Time?（s）‘）;
ylabel（‘Flux?（G）‘）;
title（‘Magnetometer‘）;
hold?off;
linkaxes（axis?‘x‘）;

%%?Process?sensor?data?through?algorithm

AHRS?=?MadgwickAHRS（‘SamplePeriod‘?1/256?‘Beta‘?0.1）;
%?AHRS?=?MahonyAHRS（‘SamplePeriod‘?1/256?‘Kp‘?0.5）;

quaternion?=?zeros（length（time）?4）;
for?t?=?1:length（time）
????AHRS.Update（Gyroscope（t:）?*?（pi/180）?Accelerometer（t:）?Magnetometer（t:））;	%?gyroscope?units?must?be?radians
????quaternion（t?:）?=?AHRS.Quaternion;
end

%%?Plot?algorithm?output?as?Euler?angles
%?The?first?and?third?Euler?angles?in?the?sequence?（phi?and?psi）?become
%?unreliable?when?the?middle?angles?of?the?sequence?（theta）?approaches??0
%?degrees.?This?problem?commonly?referred?to?as?Gimbal?Lock.
%?See:?http://en.wikipedia.org/wiki/Gimbal_lock

euler?=?quatern2euler（quaternConj（quaternion））?*?（180/pi）;	%?use?conjugate?for?sensor?frame?relative?to?Earth?and?convert?to?degrees.

figur

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????3263??2012-11-06?19:00??Examplescript.m
?????文件????????4678??2011-09-28?23:40??@MadgwickAHRS\MadgwickAHRS.m
?????文件????????4718??2011-09-28?23:40??@MahonyAHRS\MahonyAHRS.m
?????文件?????????650??2011-09-27?16:52??quaternion_library\axisAngle2quatern.m
?????文件?????????965??2011-09-27?16:52??quaternion_library\axisAngle2rotMat.m
?????文件?????????938??2011-09-27?16:52??quaternion_library\euler2rotMat.m
?????文件?????????878??2011-09-27?16:52??quaternion_library\quatern2euler.m
?????文件?????????825??2011-09-27?16:52??quaternion_library\quatern2rotMat.m
?????文件?????????378??2011-09-27?16:52??quaternion_library\quaternConj.m
?????文件?????????661??2011-09-27?16:51??quaternion_library\quaternProd.m
?????文件?????????645??2011-09-27?16:51??quaternion_library\rotMat2euler.m
?????文件????????1434??2011-09-27?16:51??quaternion_library\rotMat2quatern.m
?????文件????????2465??2011-09-27?16:51??quaternion_library\Testscript.m
?????文件??????101819??2011-09-24?13:39??ExampleData.mat