madgwick_传感器融合算法_matlab

madgwick的传感器融合算法，matlab版本的

% 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