?? orient_track.m
字號:
global mode_config
addpath 'tools'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Configuration parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mode_config.dead_reckoning=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% the number of particles
N=200;%size(imu,2);%5000;
% the initial weights
weight=1/N.*ones(1,N);
% initialize the particles
sample_ind=round(1:size(imu,2)/N:size(imu,2));
sample_rot=rot_t(sample_ind',:)';
sample_ind=1:N;
rot_seq=[];
mean_rot=zeros(1,3);
for i=2:size(a,1)
% delta t
delta_t=tstamp(i)-tstamp(i-1);
if mode_config.dead_reckoning
mean_rot=mean_rot+w(i-1,:).*delta_t;
else
% Rotation estimation
sample_rot=sample_rot+repmat((w(i-1,:).*delta_t)',1,N);
% gravity estimation
a_est=grav_est(sample_rot');
% Error calculation
a_err=sum((repmat(a(i,:),N,1)-a_est).^2,2).^.5;
% Resample cretia
if min(a_err)>0.4
weight=1/N.*ones(1,N);
sample_ind=round(1:size(imu,2)/N:size(imu,2));
sample_rot=rot_t(sample_ind,:)';
sample_ind=1:N;
end
% weight set according to the error
weight=1./a_err;
% normalize the weights
weight=weight./sum(weight);
% particle resample
sample_ind=residualR(sample_ind,weight);
sample_rot=sample_rot(:,sample_ind);%rot_t(sample_ind,:)';
% estimation set to the weighted sum of the particles
mean_rot=[sum(sample_rot(1,:)'.*weight),sum(sample_rot(2,:)'.*weight),sum(sample_rot(3,:)'.*weight)];
% save the sequence for analysis
rot_seq=[rot_seq;mean_rot];
end
% plot the result
plot_contrast(vicon,mean_rot,sample_rot,test_vicon(1,i).R,i);
end
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -