-
Notifications
You must be signed in to change notification settings - Fork 0
/
MahonyAHRS.m
107 lines (90 loc) · 4.61 KB
/
MahonyAHRS.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
classdef MahonyAHRS < handle
%MAYHONYAHRS Madgwick's implementation of Mayhony's AHRS algorithm
%
% For more information see:
% http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
%
% Date Author Notes
% 28/09/2011 SOH Madgwick Initial release
%% Public properties
properties (Access = public)
SamplePeriod = 1/100;
Quaternion = [1 0 0 0]; % output quaternion describing the Earth relative to the sensor
Kp = 1; % algorithm proportional gain
Ki = 0; % algorithm integral gain
end
%% Public properties
properties (Access = private)
eInt = [0 0 0]; % integral error
end
%% Public methods
methods (Access = public)
function obj = MahonyAHRS(varargin)
for i = 1:2:nargin
if strcmp(varargin{i}, 'SamplePeriod'), obj.SamplePeriod = varargin{i+1};
elseif strcmp(varargin{i}, 'Quaternion'), obj.Quaternion = varargin{i+1};
elseif strcmp(varargin{i}, 'Kp'), obj.Kp = varargin{i+1};
elseif strcmp(varargin{i}, 'Ki'), obj.Ki = varargin{i+1};
else error('Invalid argument');
end
end;
end
function obj = Update(obj, Gyroscope, Accelerometer, Magnetometer)
q = obj.Quaternion; % short name local variable for readability
% Normalise accelerometer measurement
if(norm(Accelerometer) == 0), return; end % handle NaN
Accelerometer = Accelerometer / norm(Accelerometer); % normalise magnitude
% Normalise magnetometer measurement
if(norm(Magnetometer) == 0), return; end % handle NaN
Magnetometer = Magnetometer / norm(Magnetometer); % normalise magnitude
% Reference direction of Earth's magnetic feild
h = quaternProd(q, quaternProd([0 Magnetometer], quaternConj(q)));
b = [0 norm([h(2) h(3)]) 0 h(4)];
% Estimated direction of gravity and magnetic field
v = [2*(q(2)*q(4) - q(1)*q(3))
2*(q(1)*q(2) + q(3)*q(4))
q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
w = [2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3))
2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4))
2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2)];
% Error is sum of cross product between estimated direction and measured direction of fields
e = cross(Accelerometer, v) + cross(Magnetometer, w);
if(obj.Ki > 0)
obj.eInt = obj.eInt + e * obj.SamplePeriod;
else
obj.eInt = [0 0 0];
end
% Apply feedback terms
Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;
% Compute rate of change of quaternion
qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
% Integrate to yield quaternion
q = q + qDot * obj.SamplePeriod;
obj.Quaternion = q / norm(q); % normalise quaternion
end
function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
q = obj.Quaternion; % short name local variable for readability
% Normalise accelerometer measurement
if(norm(Accelerometer) == 0), return; end % handle NaN
Accelerometer = Accelerometer / norm(Accelerometer); % normalise magnitude
% Estimated direction of gravity and magnetic flux
v = [2*(q(2)*q(4) - q(1)*q(3))
2*(q(1)*q(2) + q(3)*q(4))
q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
% Error is sum of cross product between estimated direction and measured direction of field
e = cross(Accelerometer, v);
if(obj.Ki > 0)
obj.eInt = obj.eInt + e * obj.SamplePeriod;
else
obj.eInt = [0 0 0];
end
% Apply feedback terms
Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;
% Compute rate of change of quaternion
qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
% Integrate to yield quaternion
q = q + qDot * obj.SamplePeriod;
obj.Quaternion = q / norm(q); % normalise quaternion
end
end
end