52 lines
1.6 KiB
Matlab
52 lines
1.6 KiB
Matlab
|
classdef mahony < handle
|
|||
|
|
methods (Static = true)
|
|||
|
|
function q = imu(q, gyr, acc, dt, Kp)
|
|||
|
|
|
|||
|
|
% <20><><EFBFBD>ٶȵ<D9B6>λ<EFBFBD><CEBB>
|
|||
|
|
norm_acc = norm(acc);
|
|||
|
|
if((norm_acc) == 0), return; end
|
|||
|
|
acc = acc / norm(acc);
|
|||
|
|
|
|||
|
|
qtmp = att_upt(q, gyr, dt);
|
|||
|
|
|
|||
|
|
v = qmulv(qconj(qtmp), [0 0 1]');
|
|||
|
|
|
|||
|
|
e = cross(acc, v) ;
|
|||
|
|
|
|||
|
|
% <20><><EFBFBD><EFBFBD>
|
|||
|
|
gyr = gyr + Kp * e ;
|
|||
|
|
|
|||
|
|
% <20><><EFBFBD><EFBFBD>
|
|||
|
|
q = qintg(q, gyr, dt);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|
|||
|
|
function q = ahrs(q, gyr, acc, mag, dt, Kp)
|
|||
|
|
|
|||
|
|
% <20><><EFBFBD>ٶȼƵ<C8BC>λ<EFBFBD><CEBB>
|
|||
|
|
if(norm(acc) == 0), return; end % handle NaN
|
|||
|
|
acc = acc / norm(acc); % normalise magnitude
|
|||
|
|
|
|||
|
|
% <20>ų<EFBFBD><C5B3><EFBFBD>λ<EFBFBD><CEBB>
|
|||
|
|
if(norm(mag) == 0), return; end % handle NaN
|
|||
|
|
mag = mag / norm(mag); % normalise magnitude
|
|||
|
|
|
|||
|
|
% Reference direction of Earth's magnetic feild
|
|||
|
|
h = qmulv(q, mag);
|
|||
|
|
b = [norm([h(1) h(2)]) 0 h(3)]';
|
|||
|
|
|
|||
|
|
% Estimated direction of gravity and magnetic field
|
|||
|
|
w = qmulv(qconj(q), b);
|
|||
|
|
v = qmulv(qconj(q), [0 0 1]');
|
|||
|
|
|
|||
|
|
% Error is sum of cross product between estimated direction and measured direction of fields
|
|||
|
|
e = cross(acc, v) + cross(mag, w);
|
|||
|
|
|
|||
|
|
% Apply feedback terms
|
|||
|
|
gyr = gyr + Kp * e ;
|
|||
|
|
|
|||
|
|
% integate
|
|||
|
|
q = qintg(q, gyr, dt);
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
end
|