Professional Documents
Culture Documents
Mahony AHRS
Mahony AHRS
%% Public properties
properties (Access = public)
SamplePeriod = 1/256;
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