forked from ZhanLi/UWBIns
45 lines
979 B
Mathematica
45 lines
979 B
Mathematica
|
|
|||
|
|
function out = att_upt(in, gyr, dt)
|
|||
|
|
% <EFBFBD><EFBFBD><EFBFBD>뵱ǰ<EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>º<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
% inΪ<EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
% gyr<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Dz<EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
% dt ʱ<EFBFBD>䲽<EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
%% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תʸ<EFBFBD><EFBFBD>
|
|||
|
|
rv = gyr*dt; % <EFBFBD><EFBFBD>תʸ<EFBFBD><EFBFBD> = <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <EFBFBD><EFBFBD> ʱ<EFBFBD>䲽<EFBFBD><EFBFBD>
|
|||
|
|
dq = rv2q(rv); % <EFBFBD><EFBFBD><EFBFBD>ú<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תʸ<EFBFBD><EFBFBD>תΪ<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
%% <EFBFBD><EFBFBD>רҵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
% dq(1) = 1;
|
|||
|
|
% dq(2) = rv(1)*0.5;
|
|||
|
|
% dq(3) = rv(2)*0.5;
|
|||
|
|
% dq(4) = rv(3)*0.5;
|
|||
|
|
|
|||
|
|
out = qmul(in, dq); % <EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD>̬ <EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD> = <EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬
|
|||
|
|
out = qnormlz(out); % <EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><EFBFBD>ֵƯ<EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
%% ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
%
|
|||
|
|
% Cb2n = ch_q2m(in);
|
|||
|
|
% theta = gyr*dt;
|
|||
|
|
%
|
|||
|
|
% %C = eye(3) + ch_askew(theta);
|
|||
|
|
% C = ch_rv2m(theta);
|
|||
|
|
%
|
|||
|
|
% Cb2n = Cb2n * C;
|
|||
|
|
%
|
|||
|
|
% % <EFBFBD>ض<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> GNSS<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD>ഫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD>ϵͳԭ<EFBFBD><EFBFBD>-<EFBFBD>ڶ<EFBFBD><EFBFBD><EFBFBD>.pdf <EFBFBD><EFBFBD>ʽ 5.80
|
|||
|
|
% c1 = Cb2n(1,:);
|
|||
|
|
% c2 = Cb2n(2,:);
|
|||
|
|
% c3 = Cb2n(3,:);
|
|||
|
|
% c1 = 2 / (1 + dot(c1,c1))*c1;
|
|||
|
|
% c2 = 2 / (1 + dot(c2,c2))*c2;
|
|||
|
|
% c3 = 2 / (1 + dot(c3,c3))*c3;
|
|||
|
|
% Cb2n = [c1; c2; c3];
|
|||
|
|
%
|
|||
|
|
% out = ch_m2q(Cb2n);
|
|||
|
|
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
|