45 lines
979 B
Matlab
45 lines
979 B
Matlab
|
|
|||
|
|
function out = att_upt(in, gyr, dt)
|
|||
|
|
% <20><><EFBFBD>뵱ǰ<EBB5B1><C7B0>̬<EFBFBD><CCAC>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><C7BD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>º<EFBFBD><C2BA><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>Ԫ<EFBFBD><D4AA>
|
|||
|
|
% inΪ<6E><CEAA>ǰ<EFBFBD><C7B0>̬<EFBFBD><CCAC>Ԫ<EFBFBD><D4AA>
|
|||
|
|
% gyr<79><72><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Dz<EFBFBD><C7B2><EFBFBD>ֵ
|
|||
|
|
% dt ʱ<>䲽<EFBFBD><E4B2BD>
|
|||
|
|
|
|||
|
|
%% <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תʸ<D7AA><CAB8>
|
|||
|
|
rv = gyr*dt; % <20><>תʸ<D7AA><CAB8> = <20><><EFBFBD>ٶ<EFBFBD> <20><> ʱ<>䲽<EFBFBD><E4B2BD>
|
|||
|
|
dq = rv2q(rv); % <20><><EFBFBD>ú<EFBFBD><C3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תʸ<D7AA><CAB8>תΪ<D7AA><CEAA>Ԫ<EFBFBD><D4AA>
|
|||
|
|
|
|||
|
|
%% <20><>רҵ<D7A8><D2B5><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); % <20><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>ǰ<EFBFBD><C7B0>̬ <20><> <20><><EFBFBD><EFBFBD> = <20><><EFBFBD><EFBFBD>̬
|
|||
|
|
out = qnormlz(out); % <20><>һ<EFBFBD><D2BB> <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9>ֵƯ<D6B5><C6AF>
|
|||
|
|
|
|||
|
|
%% ʹ<><CAB9><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><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;
|
|||
|
|
%
|
|||
|
|
% % <20>ض<EFBFBD><D8B6><EFBFBD><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> GNSS<53><53><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC>ഫ<EFBFBD><E0B4AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>ϵͳԭ<CDB3><D4AD>-<2D>ڶ<EFBFBD><DAB6><EFBFBD>.pdf <20><>ʽ 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
|
|||
|
|
|